Squashed 'third_party/allwpilib_2019/' changes from 936627bd9..a3f7420da
e20d96ea4 Use __has_include for WPILib.h (#2164)
a76d006a0 Update native-utils to 2020.7.2 (#2161)
24c031d69 Increase SPI auto byte count to allow 32 bytes to be sent (#2163)
6b4eecf5f Add hidden functions to get the SPI system and SPI DMA (#2162)
ccdd0fbdb Add TrapezoidProfile external PID examples (#2131)
5c6b8a0f4 Replace std::is_pod_v with std::is_standard_layout_v (#2159)
67d2fed68 Add DutyCycleEncoder channel constructor (#2158)
d8f11eb14 Hardcode channels for LSB weight (#2153)
b2ae75acd Add way to disable "no extensions found" message (#2134)
4f951789f Build testbench tests online inorder to improve speed (#2144)
005c4c5be Try catch around task dependencies to fix loading in editor (#2156)
34f6b3f4c Fix C++ RamseteCommand param doxygen (#2157)
f7a93713f Fix up templated TrapezoidProfile classes (#2151)
8c2ff94d7 Rename MathUtils to MathUtil for consistency with other util classes (#2155)
d003ec2dc Update to 2020v9 image (#2154)
8e7cc3fe7 Add user-friendly SimDeviceSim wrappers (#2150)
6c8f6cf47 Fix bug in cubic and quintic hermetic spline generation (#2139)
e37ecd33a Sim GUI: Add support for LED displays (#2138)
57c5523d6 Fix documentation in RamseteCommand (#2149)
7b9c6ebc2 Fix wpiutilJNIShared linkage typo in wpilibj (#2143)
9a515c80f Template C++ LinearFilter to work with unit types (#2142)
5b73c17f2 Remove encoder velocities methods in DifferentialDriveOdometry (#2147)
b8c102426 Fix PS3Eye VID and PID (#2146)
2622c6c29 Add default values for b and zeta in RamseteController (#2145)
f66ae5999 Add HSV helpers to AddressableLED (#2135)
5e97c81d8 Add MedianFilter class for moving-window median (#2136)
f79b7a058 Remove unnecessary constructor arg for LinearFilter's circular buffers (#2140)
e49494830 Replace Jenkins with Azure agent (#1914)
b67d049ac Check status of PDP CAN reads (#2126)
70102a60b SendableRegistry.foreachLiveWindow: Prevent concurrent modification (#2129)
6dcd2b0e2 Improve various subsystem APIs (#2130)
ce3973435 HAL_CAN_ReceiveMessage: return MessageNotFound if no callbacks registered (#2133)
3fcfc8ea7 Fix double disable segfaulting interrupts (#2132)
6ceafe3cd Fix class reference for logger (#2123)
b058dcf64 Catch exceptions generated by OpenCV in cscore JNI (#2118)
0b9307fdf Remove unused parts of .styleguide files (#2119)
39be812b2 Fix C++ ArmFeedforward (#2120)
21e957ee4 Add DifferentialDrive voltage constraint (#2075)
e0bc97f66 Add ProfiledPIDSubsystem example (#2076)
3df44c874 Remove Rotation2d.h wpi/math include (#2117)
a58dbec8a Add holonomic follower examples (#2052)
9a8067465 Fix incomplete .styleguide (#2113)
ffa4b907c Fix C++ floating point literal formatting (#2114)
3d1ca856b Fix missing typename and return type (#2115)
5f85457a9 Add usage reporting for AddressableLED (#2108)
4ebae1712 Enforce leading/trailing zeros in Java numeric constants (#2105)
fa85fbfc1 Template C++ TrapezoidProfile and ProfiledPIDController on units (#2109)
f62e23f1a Add Doxygen for new HAL interfaces (#2110)
5443fdabc Directly construct PWM port from HAL, avoid wpilib PWM object (#2106)
c0e36df9d Standardize on PWMVictorSPX in examples (#2104)
8c4d9f541 Add TrapezoidProfileSubsystem (#2077)
45201d15f Add encoder distance overload to DifferentialDriveOdometry (#2096)
845aba33f Make feedforward classes constexpr (#2103)
500c43fb8 Add examples for DMA, DutyCycle, DutyCycleEncoder and AddressableLED (#2100)
589162811 Use DifferentialDriveWheelSpeeds in RamseteCommand ctor (#2091)
b37b68daa Add JRE deployment to MyRobot Deploy (#2099)
0e83c65d2 Fix small logic error in ParallelDeadlineGroup (#2095)
6f6c6da9f Updates to addressable LED (#2098)
1894219ef Fix devmain package in commands (#2097)
77a9949bb Add AddressableLED simulation GUI support
a4c9e4ec2 Add AddressableLED simulation support
8ed205907 Add AddressableLED (#2092)
59507b12d Bump to 2020 v7 image (#2086)
5d39bf806 Make halsimgui::DrawLEDs() values parameter const (#2088)
841ef91c0 Use gyro angle instead of robot angle for odometry (#2081)
1b66ead49 Use standard constant for pi instead of 3.14 (#2084)
db2c3dddd Use DMA Global Number for DMA Index (#2085)
82b2170fe Add DMA support to HAL and WPILibC (#2080)
8280b7e3a Add DutyCycleEncoder and AnalogEncoder (#2040)
551096006 Use kNumSystems for DutyCycle count in Ports (#2083)
df1065218 Remove release configs of deploy in MyRobot (#2082)
bf5388393 Add deploy options to myRobot (#2079)
b7bc1ea74 Update to 2020v6 image (#2078)
708009cd2 Update to gradle 6.0 (#2074)
3cce61b89 Add SmartDashboard::GetEntry function in C++ (#2064)
565e1f3e7 Fix spelling in MecanumDriveOdometry docs (#2072)
1853f7b6b Add timing window to simulation GUI
c5a049712 Add simulation pause/resume/step support
f5446c740 Add Notifier HALSIM access
3e049e02f Add name to HAL Notifier
2da64d15f Make usage reporting enums type match (#2069)
f04d95e50 Make FRCUsageReporting.h C-compatible (#2070)
d748c67a5 Generate docs for command libraries and fix doclint enable (#2071)
55a7f2b4a Add template for old command-based style (#2031)
486fa9c69 Add XboxController examples for arcade and tank drive (#2058)
e3dd1c5d7 Fix small bug in SplineHelper (#2061)
7dc7c71b5 Add feedforward components (#2045)
5f33d6af1 Fix ProfiledPIDSubsystem parameter name (#2017)
94843adb8 Standardize documentation of Speed Controllers bounds (#2043)
9bcff37b9 Add HAL specific version of wpi_setError (#2055)
326aecc9a Add error message for CAN Write Overrun (#2062)
00228678d Add requirements param to more Command APIs (#2059)
ff39a96ce Make DigitalOutput a DigitalSource (#2054)
5ccad2e8a Fix frc2::Timer returning incorrect timestamp values (#2057)
629e95776 Add VendorDeps JSON files for command libraries (#2048)
6858a57f7 Make notifier command tests a lot more lenient (#2056)
0ebe32823 Fix MyRobotStatic accidentally linking to shared command libs (#2046)
384d00f9e Fix various duty cycle bugs (#2047)
1f6850adf Add CAN Manufacturer for Studica (#2050)
7508aada9 Add ability to end startCompetition() main loop (#2032)
f5b4be16d Old C++ Command: Make GetName et al public (#2042)
e6f5c93ab Clean up new C++ commands (#2027)
39f46ceab Don't allow rising and falling values to be read from AnalogTrigger (#2039)
d93aa2b6b Add missing lock in FRCDriverStation (#2034)
114ddaf81 Fix duplicate encoders in examples (#2033)
f22d0961e Sim GUI: Add duty cycle support
3262c2bad Sim GUI: Use new multi-channel PDP getter function
96d40192a Revert accidental change to MyRobot.cpp (#2029)
ed30d5d40 Add JSON support for Trajectories (#2025)
2b6811edd Fix null pointer dereference in C++ CommandScheduler (#2023)
1d695a166 Add FPGA Duty Cycle support (#1987)
509819d83 Split the two command implementations into separate libraries (#2012)
2ad15cae1 Add multi PDP getter and sim PCM/PDP multi arg functions (#2014)
931b8ceef Add new usage reporting types from 2020v5 (#2026)
0b3821eba Change files with CRLF line endings to LF (#2022)
6f159d142 Add way to atomically check for new data, and wait otherwise (#2015)
a769f1f22 Fix bug in RamseteCommand (using degrees instead of radians) (#2020)
c5186d815 Clean up PIDCommand (#2010)
9ebd23d61 Add setVoltage method to SpeedController (#1997)
f6e311ef8 Fix SplineHelper bug (#2018)
f33bd9f05 Fix NPE in RamseteCommand (#2019)
1c1e0c9a6 Add HAL_SetAllSolenoids to sim (#2004)
ea9bb651a Remove accidental OpenCV link from wpilibc shared library (#2013)
cc0742518 Change command decorators to return implementation (#2007)
16b34cce2 Remove IterativeRobot templates (#2011)
669127e49 Update intellisense to work with Beta 2020 code (#2008)
9dc30797e Fix usage reporting indices (#2009)
f6b844ea3 Move HAL Interrupt struct to anonymous namespace (#2003)
a72f80991 Add extern C to DigitalGlitchFilterJNI (#2002)
916596cb0 Fix invalid examples json, add validator (#2001)
5509a8e96 Use constexpr for all example constants
0be6b6475 Use constexpr for DifferentialDriveKinematics
Change-Id: I1416646cbab487ad8021830215766d8ec7f24ddc
git-subtree-dir: third_party/allwpilib_2019
git-subtree-split: a3f7420dab7a104c27a0c3bf0872c999c98fd9a9
diff --git a/.gitignore b/.gitignore
index 85c56af..bdf94fe 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,6 +3,7 @@
dependency-reduced-pom.xml
doxygen.log
build*/
+!buildSrc/
# Created by the jenkins test script
test-reports
diff --git a/.styleguide b/.styleguide
index aaeda49..d465d74 100644
--- a/.styleguide
+++ b/.styleguide
@@ -9,18 +9,10 @@
}
generatedFileExclude {
- gtest/
- ni-libraries/include/
- ni-libraries/lib/
FRCNetComm\.java$
- simulation/frc_gazebo_plugins/frcgazebo/
simulation/gz_msgs/src/include/simulation/gz_msgs/msgs\.h$
}
-modifiableFileExclude {
- \.so$
-}
-
repoRootNameOverride {
wpilib
}
diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json
index 7e9049c..984952e 100644
--- a/.wpilib/wpilib_preferences.json
+++ b/.wpilib/wpilib_preferences.json
@@ -1,6 +1,6 @@
{
"enableCppIntellisense": true,
"currentLanguage": "cpp",
- "projectYear": "2019",
+ "projectYear": "Beta2020-2",
"teamNumber": 0
}
diff --git a/azure-pipelines-testbench.yaml b/azure-pipelines-testbench.yaml
index 3ed5507..67cd75c 100644
--- a/azure-pipelines-testbench.yaml
+++ b/azure-pipelines-testbench.yaml
@@ -1,19 +1,91 @@
-# Starter pipeline
-# Start with a minimal pipeline that you can customize to build and deploy your code.
-# Add steps that build, run tests, deploy, and more:
-# https://aka.ms/yaml
-
-trigger:
-- master
-
-pool:
- vmImage: 'ubuntu-latest'
-
-steps:
-- script: echo Hello, world!
- displayName: 'Run a one-line script'
-
-- script: |
- echo Add other tasks to build, test, and deploy your project.
- echo See https://aka.ms/yaml
- displayName: 'Run a multi-line script'
+# Testing steps for real hardware
+
+trigger:
+ batch: true
+ branches:
+ include:
+ - master
+
+stages:
+- stage: Build
+ jobs:
+ - job: IntegrationTests
+ displayName: Integration Tests
+ pool:
+ vmImage: 'Ubuntu 16.04'
+
+ container:
+ image: wpilib/roborio-cross-ubuntu:2020-18.04
+
+ timeoutInMinutes: 0
+
+ steps:
+ - task: Gradle@2
+ condition: and(succeeded(), not(startsWith(variables['Build.SourceBranch'], 'refs/tags/v')))
+ inputs:
+ workingDirectory: ''
+ gradleWrapperFile: 'gradlew'
+ gradleOptions: '-Xmx3072m'
+ publishJUnitResults: false
+ testResultsFiles: '**/TEST-*.xml'
+ tasks: 'copyWpilibJIntegrationTestJarToOutput copyWpilibCTestLibrariesToOutput'
+ options: '-Ponlylinuxathena -PbuildServer'
+
+ - task: PublishPipelineArtifact@0
+ inputs:
+ artifactName: 'Integration Tests'
+ targetPath: 'build/integrationTestFiles'
+
+- stage: TestBench
+ displayName: Test Bench
+ jobs:
+ - job: Cpp
+ displayName: C++
+ pool: RoboRioConnections
+ timeoutInMinutes: 30
+ workspace:
+ clean: all
+ steps:
+ - task: DownloadPipelineArtifact@0
+ inputs:
+ artifactName: 'Integration Tests'
+ targetPath: 'build/integrationTestFiles'
+
+ - task: ShellScript@2
+ displayName: Run C++ Tests
+ inputs:
+ scriptPath: test-scripts/deploy-and-run-test-on-robot.sh
+ args: 'cpp -A "--gtest_output=xml:/home/admin/testResults/cppreport.xml"'
+
+ - task: PublishTestResults@2
+ displayName: Publish C++ Test Results
+ inputs:
+ testResultsFormat: 'JUnit'
+ testResultsFiles: '*.xml'
+ testRunTitle: 'C++ Test Report'
+ searchFolder: '$(System.DefaultWorkingDirectory)/test-reports'
+
+ - job: Java
+ pool: RoboRioConnections
+ timeoutInMinutes: 30
+ workspace:
+ clean: all
+ steps:
+ - task: DownloadPipelineArtifact@0
+ inputs:
+ artifactName: 'Integration Tests'
+ targetPath: 'build/integrationTestFiles'
+
+ - task: ShellScript@2
+ displayName: Run Java Tests
+ inputs:
+ scriptPath: test-scripts/deploy-and-run-test-on-robot.sh
+ args: 'java'
+
+ - task: PublishTestResults@2
+ displayName: Publish Java Test Results
+ inputs:
+ testResultsFormat: 'JUnit'
+ testResultsFiles: '*.xml'
+ testRunTitle: 'Java Test Report'
+ searchFolder: '$(System.DefaultWorkingDirectory)/test-reports'
diff --git a/build.gradle b/build.gradle
index b6b6410..81b333f 100644
--- a/build.gradle
+++ b/build.gradle
@@ -3,15 +3,14 @@
plugins {
id 'base'
id 'edu.wpi.first.wpilib.versioning.WPILibVersioningPlugin' version '4.0.1'
- id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' version '2020.1'
+ id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' version '2020.2'
id 'edu.wpi.first.NativeUtils' apply false
- id 'edu.wpi.first.GradleJni' version '0.9.1'
- id 'edu.wpi.first.GradleVsCode' version '0.9.4'
+ id 'edu.wpi.first.GradleJni' version '0.10.1'
+ id 'edu.wpi.first.GradleVsCode' version '0.11.0'
id 'idea'
id 'visual-studio'
- id 'com.gradle.build-scan' version '2.3'
- id 'net.ltgt.errorprone' version '0.6' apply false
- id 'com.github.johnrengelman.shadow' version '4.0.3' apply false
+ id 'net.ltgt.errorprone' version '1.1.1' apply false
+ id 'com.github.johnrengelman.shadow' version '5.2.0' apply false
}
if (project.hasProperty('buildServer')) {
@@ -99,7 +98,9 @@
// Disables doclint in java 8.
if (JavaVersion.current().isJava8Compatible()) {
tasks.withType(Javadoc) {
- options.addStringOption('Xdoclint:none', '-quiet')
+ if (project.name != "docs") {
+ options.addStringOption('Xdoclint:none', '-quiet')
+ }
}
}
}
@@ -109,5 +110,5 @@
}
wrapper {
- gradleVersion = '5.4.1'
+ gradleVersion = '6.0'
}
diff --git a/buildSrc/build.gradle b/buildSrc/build.gradle
index a0c2b85..b4a6a01 100644
--- a/buildSrc/build.gradle
+++ b/buildSrc/build.gradle
@@ -5,5 +5,5 @@
}
}
dependencies {
- compile "edu.wpi.first:native-utils:2020.1.4"
+ implementation "edu.wpi.first:native-utils:2020.7.2"
}
diff --git a/buildSrc/src/main/groovy/JREArtifact.groovy b/buildSrc/src/main/groovy/JREArtifact.groovy
new file mode 100644
index 0000000..e57249a
--- /dev/null
+++ b/buildSrc/src/main/groovy/JREArtifact.groovy
@@ -0,0 +1,48 @@
+import groovy.transform.CompileStatic
+import javax.inject.Inject
+import jaci.gradle.deploy.artifact.MavenArtifact
+import jaci.gradle.deploy.context.DeployContext
+import org.gradle.api.Project
+import jaci.gradle.ActionWrapper
+
+import java.util.function.Function
+
+@CompileStatic
+class JREArtifact extends MavenArtifact {
+ Function<DeployContext, Boolean> buildRequiresJre = (Function<DeployContext, Boolean>){ true }
+
+ void setJreDependency(String dep) {
+ dependency = project.dependencies.add(configuration(), dep)
+ }
+
+ @Inject
+ JREArtifact(String name, Project project) {
+ super(name, project)
+ configuration = project.configurations.create(configuration())
+
+ onlyIf = { DeployContext ctx ->
+ (buildRequiresJre.apply(ctx) && jreMissing(ctx)) || project.hasProperty("force-redeploy-jre")
+ }
+
+ predeploy << new ActionWrapper({ DeployContext ctx ->
+ ctx.logger.log('Deploying RoboRIO JRE (this will take a while)...')
+ })
+
+ directory = '/tmp'
+ filename = 'frcjre.ipk'
+
+ postdeploy << new ActionWrapper({ DeployContext ctx ->
+ ctx.logger.log('Installing JRE...')
+ ctx.execute('opkg remove frc2020-openjdk*; opkg install /tmp/frcjre.ipk; rm /tmp/frcjre.ipk')
+ ctx.logger.log('JRE Deployed!')
+ })
+ }
+
+ String configuration() {
+ return name + 'frcjre'
+ }
+
+ boolean jreMissing(DeployContext ctx) {
+ return ctx.execute('if [[ -f "/usr/local/frc/JRE/bin/java" ]]; then echo OK; else echo MISSING; fi').result.contains("MISSING")
+ }
+}
diff --git a/cameraserver/.styleguide b/cameraserver/.styleguide
index 899d9c2..bea7643 100644
--- a/cameraserver/.styleguide
+++ b/cameraserver/.styleguide
@@ -7,16 +7,12 @@
\.cpp$
}
-modifiableFileExclude {
- \.so$
-}
-
repoRootNameOverride {
cameraserver
}
includeOtherLibs {
- ^HAL/
+ ^hal/
^networktables/
^opencv2/
^support/
diff --git a/cameraserver/build.gradle b/cameraserver/build.gradle
index d59d749..2f864d9 100644
--- a/cameraserver/build.gradle
+++ b/cameraserver/build.gradle
@@ -10,12 +10,12 @@
apply from: "${rootDir}/shared/javacpp/setupBuild.gradle"
dependencies {
- compile project(':wpiutil')
- compile project(':ntcore')
- compile project(':cscore')
- devCompile project(':wpiutil')
- devCompile project(':ntcore')
- devCompile project(':cscore')
+ implementation project(':wpiutil')
+ implementation project(':ntcore')
+ implementation project(':cscore')
+ devImplementation project(':wpiutil')
+ devImplementation project(':ntcore')
+ devImplementation project(':cscore')
}
ext {
diff --git a/cameraserver/multiCameraServer/build.gradle b/cameraserver/multiCameraServer/build.gradle
index c49cfb0..88a6f0e 100644
--- a/cameraserver/multiCameraServer/build.gradle
+++ b/cameraserver/multiCameraServer/build.gradle
@@ -27,12 +27,12 @@
}
dependencies {
- compile 'com.google.code.gson:gson:2.8.5'
+ implementation 'com.google.code.gson:gson:2.8.5'
- compile project(':wpiutil')
- compile project(':ntcore')
- compile project(':cscore')
- compile project(':cameraserver')
+ implementation project(':wpiutil')
+ implementation project(':ntcore')
+ implementation project(':cscore')
+ implementation project(':cameraserver')
}
model {
diff --git a/cscore/build.gradle b/cscore/build.gradle
index 4514b95..9b84031 100644
--- a/cscore/build.gradle
+++ b/cscore/build.gradle
@@ -19,7 +19,8 @@
sharedCvConfigs = [cscore : [],
cscoreBase: [],
cscoreDev : [],
- cscoreTest: []]
+ cscoreTest: [],
+ cscoreJNIShared: []]
staticCvConfigs = [cscoreJNI: []]
useJava = true
useCpp = true
diff --git a/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp b/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp
index c50a7db..52a0fea 100644
--- a/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp
+++ b/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp
@@ -5,6 +5,9 @@
/* the project. */
/*----------------------------------------------------------------------------*/
+#include <exception>
+
+#include <opencv2/core/core.hpp>
#include <wpi/SmallString.h>
#include <wpi/jni_util.h>
#include <wpi/raw_ostream.h>
@@ -33,6 +36,7 @@
static JException videoEx;
static JException nullPointerEx;
static JException unsupportedEx;
+static JException exceptionEx;
// Thread-attached environment for listener callbacks.
static JNIEnv* listenerEnv = nullptr;
@@ -45,7 +49,8 @@
static const JExceptionInit exceptions[] = {
{"edu/wpi/cscore/VideoException", &videoEx},
{"java/lang/NullPointerException", &nullPointerEx},
- {"java/lang/UnsupportedOperationException", &unsupportedEx}};
+ {"java/lang/UnsupportedOperationException", &unsupportedEx},
+ {"java/lang/Exception", &exceptionEx}};
static void ListenerOnStart() {
if (!jvm) return;
@@ -67,6 +72,30 @@
jvm->DetachCurrentThread();
}
+/// throw java exception
+static void ThrowJavaException(JNIEnv* env, const std::exception* e) {
+ wpi::SmallString<128> what;
+ jclass je = 0;
+
+ if (e) {
+ const char* exception_type = "std::exception";
+
+ if (dynamic_cast<const cv::Exception*>(e)) {
+ exception_type = "cv::Exception";
+ je = env->FindClass("org/opencv/core/CvException");
+ }
+
+ what = exception_type;
+ what += ": ";
+ what += e->what();
+ } else {
+ what = "unknown exception";
+ }
+
+ if (!je) je = exceptionEx;
+ env->ThrowNew(je, what.c_str());
+}
+
extern "C" {
JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
@@ -1096,10 +1125,16 @@
Java_edu_wpi_cscore_CameraServerCvJNI_putSourceFrame
(JNIEnv* env, jclass, jint source, jlong imageNativeObj)
{
- cv::Mat& image = *((cv::Mat*)imageNativeObj);
- CS_Status status = 0;
- cs::PutSourceFrame(source, image, &status);
- CheckStatus(env, status);
+ try {
+ cv::Mat& image = *((cv::Mat*)imageNativeObj);
+ CS_Status status = 0;
+ cs::PutSourceFrame(source, image, &status);
+ CheckStatus(env, status);
+ } catch (const std::exception& e) {
+ ThrowJavaException(env, &e);
+ } catch (...) {
+ ThrowJavaException(env, 0);
+ }
}
// int width, int height, int pixelFormat, int totalData
@@ -1555,11 +1590,19 @@
Java_edu_wpi_cscore_CameraServerCvJNI_grabSinkFrame
(JNIEnv* env, jclass, jint sink, jlong imageNativeObj)
{
- cv::Mat& image = *((cv::Mat*)imageNativeObj);
- CS_Status status = 0;
- auto rv = cs::GrabSinkFrame(sink, image, &status);
- CheckStatus(env, status);
- return rv;
+ try {
+ cv::Mat& image = *((cv::Mat*)imageNativeObj);
+ CS_Status status = 0;
+ auto rv = cs::GrabSinkFrame(sink, image, &status);
+ CheckStatus(env, status);
+ return rv;
+ } catch (const std::exception& e) {
+ ThrowJavaException(env, &e);
+ return 0;
+ } catch (...) {
+ ThrowJavaException(env, 0);
+ return 0;
+ }
}
/*
@@ -1571,11 +1614,19 @@
Java_edu_wpi_cscore_CameraServerCvJNI_grabSinkFrameTimeout
(JNIEnv* env, jclass, jint sink, jlong imageNativeObj, jdouble timeout)
{
- cv::Mat& image = *((cv::Mat*)imageNativeObj);
- CS_Status status = 0;
- auto rv = cs::GrabSinkFrameTimeout(sink, image, timeout, &status);
- CheckStatus(env, status);
- return rv;
+ try {
+ cv::Mat& image = *((cv::Mat*)imageNativeObj);
+ CS_Status status = 0;
+ auto rv = cs::GrabSinkFrameTimeout(sink, image, timeout, &status);
+ CheckStatus(env, status);
+ return rv;
+ } catch (const std::exception& e) {
+ ThrowJavaException(env, &e);
+ return 0;
+ } catch (...) {
+ ThrowJavaException(env, 0);
+ return 0;
+ }
}
static void SetRawFrameData(JNIEnv* env, jobject rawFrameObj,
diff --git a/cscore/src/main/native/linux/UsbCameraImpl.cpp b/cscore/src/main/native/linux/UsbCameraImpl.cpp
index 6bed486..a75d3cd 100644
--- a/cscore/src/main/native/linux/UsbCameraImpl.cpp
+++ b/cscore/src/main/native/linux/UsbCameraImpl.cpp
@@ -1197,7 +1197,7 @@
if (deviceNum >= 0) {
int vendorId, productId;
if (GetVendorProduct(deviceNum, &vendorId, &productId)) {
- m_ps3eyecam_exposure = vendorId == 0x2000 && productId == 0x0145;
+ m_ps3eyecam_exposure = vendorId == 0x1415 && productId == 0x2000;
}
}
}
diff --git a/docs/build.gradle b/docs/build.gradle
index c3e8cab..0505307 100644
--- a/docs/build.gradle
+++ b/docs/build.gradle
@@ -10,6 +10,8 @@
evaluationDependsOn(':cameraserver')
evaluationDependsOn(':wpilibc')
evaluationDependsOn(':wpilibj')
+evaluationDependsOn(':wpilibOldCommands')
+evaluationDependsOn(':wpilibNewCommands')
def baseArtifactIdCpp = 'documentation'
def artifactGroupIdCpp = 'edu.wpi.first.wpilibc'
@@ -29,6 +31,8 @@
cppProjectZips.add(project(':cscore').cppHeadersZip)
cppProjectZips.add(project(':cameraserver').cppHeadersZip)
cppProjectZips.add(project(':wpilibc').cppHeadersZip)
+cppProjectZips.add(project(':wpilibOldCommands').cppHeadersZip)
+cppProjectZips.add(project(':wpilibNewCommands').cppHeadersZip)
doxygen {
executables {
@@ -71,8 +75,8 @@
}
tasks.register("zipCppDocs", Zip) {
- baseName = zipBaseNameCpp
- destinationDir = outputsFolder
+ archiveBaseName = zipBaseNameCpp
+ destinationDirectory = outputsFolder
dependsOn doxygen
from ("$buildDir/docs/doxygen/html")
into '/'
@@ -100,7 +104,7 @@
classpath += project(":wpiutil").sourceSets.main.compileClasspath
options.links("https://docs.oracle.com/en/java/javase/11/docs/api/")
options.addStringOption "tag", "pre:a:Pre-Condition"
- options.addStringOption('Xdoclint:accessibility,html,missing,reference,syntax')
+ options.addBooleanOption "Xdoclint:html,missing,reference,syntax", true
options.addBooleanOption('html5', true)
dependsOn project(':wpilibj').generateJavaVersion
dependsOn project(':hal').generateUsageReporting
@@ -110,6 +114,8 @@
source project(':ntcore').sourceSets.main.java
source project(':wpilibj').sourceSets.main.java
source project(':cameraserver').sourceSets.main.java
+ source project(':wpilibOldCommands').sourceSets.main.java
+ source project(':wpilibNewCommands').sourceSets.main.java
source configurations.javaSource.collect { zipTree(it) }
include '**/*.java'
failOnError = true
@@ -130,8 +136,8 @@
}
tasks.register("zipJavaDocs", Zip) {
- baseName = zipBaseNameJava
- destinationDir = outputsFolder
+ archiveBaseName = zipBaseNameJava
+ destinationDirectory = outputsFolder
dependsOn generateJavaDocs
from ("$buildDir/docs/javadoc")
into '/'
diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar
index 5c2d1cf..cc4fdc2 100644
--- a/gradle/wrapper/gradle-wrapper.jar
+++ b/gradle/wrapper/gradle-wrapper.jar
Binary files differ
diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties
index f4d7b2b..6ce793f 100644
--- a/gradle/wrapper/gradle-wrapper.properties
+++ b/gradle/wrapper/gradle-wrapper.properties
@@ -1,5 +1,5 @@
distributionBase=GRADLE_USER_HOME
distributionPath=wrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-5.4.1-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-6.0-bin.zip
zipStoreBase=GRADLE_USER_HOME
zipStorePath=wrapper/dists
diff --git a/gradlew b/gradlew
index b0d6d0a..2fe81a7 100755
--- a/gradlew
+++ b/gradlew
@@ -7,7 +7,7 @@
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
-# http://www.apache.org/licenses/LICENSE-2.0
+# https://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
@@ -125,8 +125,8 @@
GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\""
fi
-# For Cygwin, switch paths to Windows format before running java
-if $cygwin ; then
+# For Cygwin or MSYS, switch paths to Windows format before running java
+if [ "$cygwin" = "true" -o "$msys" = "true" ] ; then
APP_HOME=`cygpath --path --mixed "$APP_HOME"`
CLASSPATH=`cygpath --path --mixed "$CLASSPATH"`
JAVACMD=`cygpath --unix "$JAVACMD"`
@@ -154,19 +154,19 @@
else
eval `echo args$i`="\"$arg\""
fi
- i=$((i+1))
+ i=`expr $i + 1`
done
case $i in
- (0) set -- ;;
- (1) set -- "$args0" ;;
- (2) set -- "$args0" "$args1" ;;
- (3) set -- "$args0" "$args1" "$args2" ;;
- (4) set -- "$args0" "$args1" "$args2" "$args3" ;;
- (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;;
- (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;;
- (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;;
- (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;;
- (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;;
+ 0) set -- ;;
+ 1) set -- "$args0" ;;
+ 2) set -- "$args0" "$args1" ;;
+ 3) set -- "$args0" "$args1" "$args2" ;;
+ 4) set -- "$args0" "$args1" "$args2" "$args3" ;;
+ 5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;;
+ 6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;;
+ 7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;;
+ 8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;;
+ 9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;;
esac
fi
@@ -175,14 +175,9 @@
for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done
echo " "
}
-APP_ARGS=$(save "$@")
+APP_ARGS=`save "$@"`
# Collect all arguments for the java command, following the shell quoting and substitution rules
eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS"
-# by default we should be in the correct project dir, but when run from Finder on Mac, the cwd is wrong
-if [ "$(uname)" = "Darwin" ] && [ "$HOME" = "$PWD" ]; then
- cd "$(dirname "$0")"
-fi
-
exec "$JAVACMD" "$@"
diff --git a/gradlew.bat b/gradlew.bat
index 15e1ee3..24467a1 100644
--- a/gradlew.bat
+++ b/gradlew.bat
@@ -5,7 +5,7 @@
@rem you may not use this file except in compliance with the License.
@rem You may obtain a copy of the License at
@rem
-@rem http://www.apache.org/licenses/LICENSE-2.0
+@rem https://www.apache.org/licenses/LICENSE-2.0
@rem
@rem Unless required by applicable law or agreed to in writing, software
@rem distributed under the License is distributed on an "AS IS" BASIS,
diff --git a/hal/build.gradle b/hal/build.gradle
index 56b220d..d744259 100644
--- a/hal/build.gradle
+++ b/hal/build.gradle
@@ -185,3 +185,14 @@
}
}
}
+
+model {
+ binaries {
+ all {
+ if (!(it instanceof NativeBinarySpec)) return
+ if (it.component.name != 'hal' && it.component.name != 'halBase') return
+ if (it.targetPlatform.name != nativeUtils.wpi.platforms.roborio) return
+ nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared')
+ }
+ }
+}
diff --git a/hal/src/generate/FRCUsageReporting.h.in b/hal/src/generate/FRCUsageReporting.h.in
index 0ed3b33..34330c8 100644
--- a/hal/src/generate/FRCUsageReporting.h.in
+++ b/hal/src/generate/FRCUsageReporting.h.in
@@ -1,14 +1,54 @@
#pragma once
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// ifdef's definition is to allow for default parameters in C++.
+#ifdef __cplusplus
+/**
+ * Reports a hardware usage to the HAL.
+ *
+ * @param resource the used resource
+ * @param instanceNumber the instance of the resource
+ * @param context a user specified context index
+ * @param feature a user specified feature string
+ * @return the index of the added value in NetComm
+ */
+int64_t HAL_Report(int32_t resource, int32_t instanceNumber,
+ int32_t context = 0, const char* feature = nullptr);
+#else
+
+/**
+ * Reports a hardware usage to the HAL.
+ *
+ * @param resource the used resource
+ * @param instanceNumber the instance of the resource
+ * @param context a user specified context index
+ * @param feature a user specified feature string
+ * @return the index of the added value in NetComm
+ */
+int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context,
+ const char* feature);
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
/*
* Autogenerated file! Do not manually edit this file.
*/
+#ifdef __cplusplus
namespace HALUsageReporting {
- typedef enum {
+ enum tResourceType : int32_t {
${usage_reporting_types_cpp}
- } tResourceType;
- typedef enum {
+ };
+ enum tInstances : int32_t {
${usage_reporting_instances_cpp}
- } tInstances;
+ };
}
+#endif
diff --git a/hal/src/generate/ResourceType.txt b/hal/src/generate/ResourceType.txt
index 1c9068a..4ee8eb2 100644
--- a/hal/src/generate/ResourceType.txt
+++ b/hal/src/generate/ResourceType.txt
@@ -62,7 +62,7 @@
kResourceType_PigeonIMU = 61
kResourceType_NidecBrushless = 62
kResourceType_CANifier = 63
-kResourceType_CTRE_future0 = 64
+kResourceType_TalonFX = 64
kResourceType_CTRE_future1 = 65
kResourceType_CTRE_future2 = 66
kResourceType_CTRE_future3 = 67
@@ -83,3 +83,12 @@
kResourceType_RevSparkMaxPWM = 82
kResourceType_RevSparkMaxCAN = 83
kResourceType_ADIS16470 = 84
+kResourceType_PIDController2 = 85
+kResourceType_ProfiledPIDController = 86
+kResourceType_Kinematics = 87
+kResourceType_Odometry = 88
+kResourceType_Units = 89
+kResourceType_TrapezoidProfile = 90
+kResourceType_DutyCycle = 91
+kResourceType_AddressableLEDs = 92
+kResourceType_FusionVenom = 93
diff --git a/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java b/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java
new file mode 100644
index 0000000..fb34996
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* 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.hal;
+
+@SuppressWarnings("AbbreviationAsWordInName")
+public class AddressableLEDJNI extends JNIWrapper {
+ public static native int initialize(int pwmHandle);
+ public static native void free(int handle);
+
+ public static native void setLength(int handle, int length);
+ public static native void setData(int handle, byte[] data);
+
+ public static native void setBitTiming(int handle, int lowTime0, int highTime0, int lowTime1, int highTime1);
+ public static native void setSyncTime(int handle, int syncTime);
+
+ public static native void start(int handle);
+ public static native void stop(int handle);
+}
diff --git a/hal/src/main/java/edu/wpi/first/hal/AnalogJNI.java b/hal/src/main/java/edu/wpi/first/hal/AnalogJNI.java
index 38e67a7..449c065 100644
--- a/hal/src/main/java/edu/wpi/first/hal/AnalogJNI.java
+++ b/hal/src/main/java/edu/wpi/first/hal/AnalogJNI.java
@@ -7,8 +7,6 @@
package edu.wpi.first.hal;
-import java.nio.IntBuffer;
-
public class AnalogJNI extends JNIWrapper {
/**
* <i>native declaration : AthenaJava\target\native\include\HAL\Analog.h:58</i><br> enum values
@@ -94,13 +92,18 @@
public static native void getAccumulatorOutput(int analogPortHandle, AccumulatorResult result);
- public static native int initializeAnalogTrigger(int analogInputHandle, IntBuffer index);
+ public static native int initializeAnalogTrigger(int analogInputHandle);
+
+ public static native int initializeAnalogTriggerDutyCycle(int dutyCycleHandle);
public static native void cleanAnalogTrigger(int analogTriggerHandle);
public static native void setAnalogTriggerLimitsRaw(int analogTriggerHandle, int lower,
int upper);
+ public static native void setAnalogTriggerLimitsDutyCycle(int analogTriggerHandle, double lower,
+ double higher);
+
public static native void setAnalogTriggerLimitsVoltage(int analogTriggerHandle,
double lower, double upper);
@@ -115,4 +118,7 @@
public static native boolean getAnalogTriggerTriggerState(int analogTriggerHandle);
public static native boolean getAnalogTriggerOutput(int analogTriggerHandle, int type);
+
+ @SuppressWarnings("AbbreviationAsWordInName")
+ public static native int getAnalogTriggerFPGAIndex(int analogTriggerHandle);
}
diff --git a/hal/src/main/java/edu/wpi/first/hal/DutyCycleJNI.java b/hal/src/main/java/edu/wpi/first/hal/DutyCycleJNI.java
new file mode 100644
index 0000000..f0f93e1
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/DutyCycleJNI.java
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* 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.hal;
+
+public class DutyCycleJNI extends JNIWrapper {
+ public static native int initialize(int digitalSourceHandle, int analogTriggerType);
+ public static native void free(int handle);
+
+ public static native int getFrequency(int handle);
+ public static native double getOutput(int handle);
+ public static native int getOutputRaw(int handle);
+ public static native int getOutputScaleFactor(int handle);
+
+ @SuppressWarnings("AbbreviationAsWordInName")
+ public static native int getFPGAIndex(int handle);
+}
diff --git a/hal/src/main/java/edu/wpi/first/hal/NotifierJNI.java b/hal/src/main/java/edu/wpi/first/hal/NotifierJNI.java
index bf92c37..000c7d6 100644
--- a/hal/src/main/java/edu/wpi/first/hal/NotifierJNI.java
+++ b/hal/src/main/java/edu/wpi/first/hal/NotifierJNI.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -20,6 +20,11 @@
public static native int initializeNotifier();
/**
+ * Sets the name of the notifier.
+ */
+ public static native void setNotifierName(int notifierHandle, String name);
+
+ /**
* Wakes up the waiter with time=0. Note: after this function is called, all
* calls to waitForNotifierAlarm() will immediately start returning 0.
*/
diff --git a/hal/src/main/java/edu/wpi/first/hal/PDPJNI.java b/hal/src/main/java/edu/wpi/first/hal/PDPJNI.java
index c45a053..e2d7fcc 100644
--- a/hal/src/main/java/edu/wpi/first/hal/PDPJNI.java
+++ b/hal/src/main/java/edu/wpi/first/hal/PDPJNI.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -21,6 +21,8 @@
public static native double getPDPChannelCurrent(byte channel, int handle);
+ public static native void getPDPAllCurrents(int handle, double[] currents);
+
public static native double getPDPTotalCurrent(int handle);
public static native double getPDPTotalPower(int handle);
diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/AddressableLEDSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/AddressableLEDSim.java
new file mode 100644
index 0000000..cc6eefa
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/sim/AddressableLEDSim.java
@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* 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.hal.sim;
+
+import edu.wpi.first.hal.sim.mockdata.AddressableLEDDataJNI;
+
+public class AddressableLEDSim {
+ private final int m_index;
+
+ public AddressableLEDSim(int index) {
+ m_index = index;
+ }
+
+ public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+ int uid = AddressableLEDDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+ return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelInitializedCallback);
+ }
+ public boolean getInitialized() {
+ return AddressableLEDDataJNI.getInitialized(m_index);
+ }
+ public void setInitialized(boolean initialized) {
+ AddressableLEDDataJNI.setInitialized(m_index, initialized);
+ }
+
+ public CallbackStore registerOutputPortCallback(NotifyCallback callback, boolean initialNotify) {
+ int uid = AddressableLEDDataJNI.registerOutputPortCallback(m_index, callback, initialNotify);
+ return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelOutputPortCallback);
+ }
+ public int getOutputPort() {
+ return AddressableLEDDataJNI.getOutputPort(m_index);
+ }
+ public void setOutputPort(int outputPort) {
+ AddressableLEDDataJNI.setOutputPort(m_index, outputPort);
+ }
+
+ public CallbackStore registerLengthCallback(NotifyCallback callback, boolean initialNotify) {
+ int uid = AddressableLEDDataJNI.registerLengthCallback(m_index, callback, initialNotify);
+ return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelLengthCallback);
+ }
+ public int getLength() {
+ return AddressableLEDDataJNI.getLength(m_index);
+ }
+ public void setLength(int length) {
+ AddressableLEDDataJNI.setLength(m_index, length);
+ }
+
+ public CallbackStore registerRunningCallback(NotifyCallback callback, boolean initialNotify) {
+ int uid = AddressableLEDDataJNI.registerRunningCallback(m_index, callback, initialNotify);
+ return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelRunningCallback);
+ }
+ public boolean getRunning() {
+ return AddressableLEDDataJNI.getRunning(m_index);
+ }
+ public void setRunning(boolean running) {
+ AddressableLEDDataJNI.setRunning(m_index, running);
+ }
+
+ public CallbackStore registerDataCallback(ConstBufferCallback callback) {
+ int uid = AddressableLEDDataJNI.registerDataCallback(m_index, callback);
+ return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelDataCallback);
+ }
+ public byte[] getData() {
+ return AddressableLEDDataJNI.getData(m_index);
+ }
+ public void setData(byte[] data) {
+ AddressableLEDDataJNI.setData(m_index, data);
+ }
+
+ public void resetData() {
+ AddressableLEDDataJNI.resetData(m_index);
+ }
+}
diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/DutyCycleSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/DutyCycleSim.java
new file mode 100644
index 0000000..9b2806b
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/sim/DutyCycleSim.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* 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.hal.sim;
+
+import edu.wpi.first.hal.sim.mockdata.DutyCycleDataJNI;
+
+public class DutyCycleSim {
+ private final int m_index;
+
+ public DutyCycleSim(int index) {
+ m_index = index;
+ }
+
+ public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+ int uid = DutyCycleDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+ return new CallbackStore(m_index, uid, DutyCycleDataJNI::cancelInitializedCallback);
+ }
+ public boolean getInitialized() {
+ return DutyCycleDataJNI.getInitialized(m_index);
+ }
+ public void setInitialized(boolean initialized) {
+ DutyCycleDataJNI.setInitialized(m_index, initialized);
+ }
+
+ public CallbackStore registerFrequencyCallback(NotifyCallback callback, boolean initialNotify) {
+ int uid = DutyCycleDataJNI.registerFrequencyCallback(m_index, callback, initialNotify);
+ return new CallbackStore(m_index, uid, DutyCycleDataJNI::cancelFrequencyCallback);
+ }
+ public int getFrequency() {
+ return DutyCycleDataJNI.getFrequency(m_index);
+ }
+ public void setFrequency(int frequency) {
+ DutyCycleDataJNI.setFrequency(m_index, frequency);
+ }
+
+ public CallbackStore registerOutputCallback(NotifyCallback callback, boolean initialNotify) {
+ int uid = DutyCycleDataJNI.registerOutputCallback(m_index, callback, initialNotify);
+ return new CallbackStore(m_index, uid, DutyCycleDataJNI::cancelOutputCallback);
+ }
+ public double getOutput() {
+ return DutyCycleDataJNI.getOutput(m_index);
+ }
+ public void setOutput(double output) {
+ DutyCycleDataJNI.setOutput(m_index, output);
+ }
+}
diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/NotifierSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/NotifierSim.java
new file mode 100644
index 0000000..f19a674
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/sim/NotifierSim.java
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* 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.hal.sim;
+
+import edu.wpi.first.hal.sim.mockdata.NotifierDataJNI;
+
+public final class NotifierSim {
+ private NotifierSim() {
+ }
+
+ public static long getNextTimeout() {
+ return NotifierDataJNI.getNextTimeout();
+ }
+
+ public static int getNumNotifiers() {
+ return NotifierDataJNI.getNumNotifiers();
+ }
+}
diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/SimDeviceSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/SimDeviceSim.java
new file mode 100644
index 0000000..e46c811
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/sim/SimDeviceSim.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.hal.sim;
+
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.hal.SimEnum;
+import edu.wpi.first.hal.SimValue;
+import edu.wpi.first.hal.sim.mockdata.SimDeviceDataJNI;
+
+public class SimDeviceSim {
+ private final int m_handle;
+
+ public SimDeviceSim(String name) {
+ m_handle = SimDeviceDataJNI.getSimDeviceHandle(name);
+ }
+
+ public SimValue getValue(String name) {
+ int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+ if (handle <= 0) {
+ return null;
+ }
+ return new SimValue(handle);
+ }
+
+ public SimDouble getDouble(String name) {
+ int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+ if (handle <= 0) {
+ return null;
+ }
+ return new SimDouble(handle);
+ }
+
+ public SimEnum getEnum(String name) {
+ int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+ if (handle <= 0) {
+ return null;
+ }
+ return new SimEnum(handle);
+ }
+
+ public SimBoolean getBoolean(String name) {
+ int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+ if (handle <= 0) {
+ return null;
+ }
+ return new SimBoolean(handle);
+ }
+
+ public static String[] getEnumOptions(SimEnum val) {
+ return SimDeviceDataJNI.getSimValueEnumOptions(val.getNativeHandle());
+ }
+
+ public SimDeviceDataJNI.SimValueInfo[] enumerateValues() {
+ return SimDeviceDataJNI.enumerateSimValues(m_handle);
+ }
+
+ public int getNativeHandle() {
+ return m_handle;
+ }
+
+ public CallbackStore registerValueCreatedCallback(SimValueCallback callback, boolean initialNotify) {
+ int uid = SimDeviceDataJNI.registerSimValueCreatedCallback(m_handle, callback, initialNotify);
+ return new CallbackStore(uid, SimDeviceDataJNI::cancelSimValueCreatedCallback);
+ }
+
+ public CallbackStore registerValueChangedCallback(SimValueCallback callback, boolean initialNotify) {
+ int uid = SimDeviceDataJNI.registerSimValueChangedCallback(m_handle, callback, initialNotify);
+ return new CallbackStore(uid, SimDeviceDataJNI::cancelSimValueChangedCallback);
+ }
+
+ public static SimDeviceDataJNI.SimDeviceInfo[] enumerateDevices(String prefix) {
+ return SimDeviceDataJNI.enumerateSimDevices(prefix);
+ }
+
+ public CallbackStore registerDeviceCreatedCallback(String prefix, SimDeviceCallback callback, boolean initialNotify) {
+ int uid = SimDeviceDataJNI.registerSimDeviceCreatedCallback(prefix, callback, initialNotify);
+ return new CallbackStore(uid, SimDeviceDataJNI::cancelSimDeviceCreatedCallback);
+ }
+
+ public CallbackStore registerDeviceFreedCallback(String prefix, SimDeviceCallback callback) {
+ int uid = SimDeviceDataJNI.registerSimDeviceFreedCallback(prefix, callback);
+ return new CallbackStore(uid, SimDeviceDataJNI::cancelSimDeviceFreedCallback);
+ }
+
+ public static void resetData() {
+ SimDeviceDataJNI.resetSimDeviceData();
+ }
+}
diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/SimHooks.java b/hal/src/main/java/edu/wpi/first/hal/sim/SimHooks.java
index 6c46111..9805497 100644
--- a/hal/src/main/java/edu/wpi/first/hal/sim/SimHooks.java
+++ b/hal/src/main/java/edu/wpi/first/hal/sim/SimHooks.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -24,4 +24,20 @@
public static void restartTiming() {
SimulatorJNI.restartTiming();
}
+
+ public static void pauseTiming() {
+ SimulatorJNI.pauseTiming();
+ }
+
+ public static void resumeTiming() {
+ SimulatorJNI.resumeTiming();
+ }
+
+ public static boolean isTimingPaused() {
+ return SimulatorJNI.isTimingPaused();
+ }
+
+ public static void stepTiming(long delta) {
+ SimulatorJNI.stepTiming(delta);
+ }
}
diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AddressableLEDDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AddressableLEDDataJNI.java
new file mode 100644
index 0000000..8098e68
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AddressableLEDDataJNI.java
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* 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.hal.sim.mockdata;
+
+import edu.wpi.first.hal.sim.ConstBufferCallback;
+import edu.wpi.first.hal.sim.NotifyCallback;
+import edu.wpi.first.hal.JNIWrapper;
+
+public class AddressableLEDDataJNI extends JNIWrapper {
+ public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+ public static native void cancelInitializedCallback(int index, int uid);
+ public static native boolean getInitialized(int index);
+ public static native void setInitialized(int index, boolean initialized);
+
+ public static native int registerOutputPortCallback(int index, NotifyCallback callback, boolean initialNotify);
+ public static native void cancelOutputPortCallback(int index, int uid);
+ public static native int getOutputPort(int index);
+ public static native void setOutputPort(int index, int outputPort);
+
+ public static native int registerLengthCallback(int index, NotifyCallback callback, boolean initialNotify);
+ public static native void cancelLengthCallback(int index, int uid);
+ public static native int getLength(int index);
+ public static native void setLength(int index, int length);
+
+ public static native int registerRunningCallback(int index, NotifyCallback callback, boolean initialNotify);
+ public static native void cancelRunningCallback(int index, int uid);
+ public static native boolean getRunning(int index);
+ public static native void setRunning(int index, boolean running);
+
+ public static native int registerDataCallback(int index, ConstBufferCallback callback);
+ public static native void cancelDataCallback(int index, int uid);
+ public static native byte[] getData(int index);
+ public static native void setData(int index, byte[] data);
+
+ public static native void resetData(int index);
+}
diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DutyCycleDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DutyCycleDataJNI.java
new file mode 100644
index 0000000..5cbae6e
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DutyCycleDataJNI.java
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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.hal.sim.mockdata;
+
+import edu.wpi.first.hal.sim.NotifyCallback;
+import edu.wpi.first.hal.JNIWrapper;
+
+public class DutyCycleDataJNI extends JNIWrapper {
+ public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+ public static native void cancelInitializedCallback(int index, int uid);
+ public static native boolean getInitialized(int index);
+ public static native void setInitialized(int index, boolean initialized);
+
+ public static native int registerFrequencyCallback(int index, NotifyCallback callback, boolean initialNotify);
+ public static native void cancelFrequencyCallback(int index, int uid);
+ public static native int getFrequency(int index);
+ public static native void setFrequency(int index, int frequency);
+
+ public static native int registerOutputCallback(int index, NotifyCallback callback, boolean initialNotify);
+ public static native void cancelOutputCallback(int index, int uid);
+ public static native double getOutput(int index);
+ public static native void setOutput(int index, double output);
+
+ public static native void resetData(int index);
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/NotifierDataJNI.java
similarity index 64%
copy from wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
copy to hal/src/main/java/edu/wpi/first/hal/sim/mockdata/NotifierDataJNI.java
index 8bd62ea..ecc0842 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
+++ b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/NotifierDataJNI.java
@@ -5,12 +5,11 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include "frc2/command/PrintCommand.h"
+package edu.wpi.first.hal.sim.mockdata;
-using namespace frc2;
+import edu.wpi.first.hal.JNIWrapper;
-PrintCommand::PrintCommand(const wpi::Twine& message)
- : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
+public class NotifierDataJNI extends JNIWrapper {
+ public static native long getNextTimeout();
+ public static native int getNumNotifiers();
}
-
-bool PrintCommand::RunsWhenDisabled() const { return true; }
diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SimulatorJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SimulatorJNI.java
index bb86005..bfd8505 100644
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SimulatorJNI.java
+++ b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SimulatorJNI.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -13,5 +13,9 @@
public static native void waitForProgramStart();
public static native void setProgramStarted();
public static native void restartTiming();
+ public static native void pauseTiming();
+ public static native void resumeTiming();
+ public static native boolean isTimingPaused();
+ public static native void stepTiming(long delta);
public static native void resetHandles();
}
diff --git a/hal/src/main/native/athena/AddressableLED.cpp b/hal/src/main/native/athena/AddressableLED.cpp
new file mode 100644
index 0000000..9ecc4c4
--- /dev/null
+++ b/hal/src/main/native/athena/AddressableLED.cpp
@@ -0,0 +1,286 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/AddressableLED.h"
+
+#include <cstring>
+
+#include "ConstantsInternal.h"
+#include "DigitalInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/ChipObject.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+
+using namespace hal;
+
+extern "C" {
+NiFpga_Status NiFpga_ClientFunctionCall(NiFpga_Session session, uint32_t group,
+ uint32_t functionId,
+ const void* inBuffer,
+ size_t inBufferSize, void* outBuffer,
+ size_t outBufferSize);
+} // extern "C"
+
+namespace {
+struct AddressableLED {
+ std::unique_ptr<tLED> led;
+ void* ledBuffer;
+ size_t ledBufferSize;
+ int32_t stringLength = 1;
+};
+} // namespace
+
+static LimitedHandleResource<
+ HAL_AddressableLEDHandle, AddressableLED, kNumAddressableLEDs,
+ HAL_HandleEnum::AddressableLED>* addressableLEDHandles;
+
+namespace hal {
+namespace init {
+void InitializeAddressableLED() {
+ static LimitedHandleResource<HAL_AddressableLEDHandle, AddressableLED,
+ kNumAddressableLEDs,
+ HAL_HandleEnum::AddressableLED>
+ alH;
+ addressableLEDHandles = &alH;
+}
+} // namespace init
+} // namespace hal
+
+// Shim for broken ChipObject function
+static const uint32_t clientFeature_hostMemoryBuffer = 0;
+static const uint32_t hostMemoryBufferFunction_open = 2;
+
+// Input arguments for HMB open
+struct AtomicHMBOpenInputs {
+ const char* memoryName;
+};
+
+// Output arguments for HMB open
+struct AtomicHMBOpenOutputs {
+ size_t size;
+ void* virtualAddress;
+};
+
+static NiFpga_Status OpenHostMemoryBuffer(NiFpga_Session session,
+ const char* memoryName,
+ void** virtualAddress, size_t* size) {
+ struct AtomicHMBOpenOutputs outputs;
+
+ struct AtomicHMBOpenInputs inputs;
+ inputs.memoryName = memoryName;
+
+ NiFpga_Status retval = NiFpga_ClientFunctionCall(
+ session, clientFeature_hostMemoryBuffer, hostMemoryBufferFunction_open,
+ &inputs, sizeof(struct AtomicHMBOpenInputs), &outputs,
+ sizeof(struct AtomicHMBOpenOutputs));
+ if (NiFpga_IsError(retval)) {
+ return retval;
+ }
+ *virtualAddress = outputs.virtualAddress;
+ if (size != NULL) {
+ *size = outputs.size;
+ }
+ return retval;
+}
+
+extern "C" {
+
+HAL_AddressableLEDHandle HAL_InitializeAddressableLED(
+ HAL_DigitalHandle outputPort, int32_t* status) {
+ hal::init::CheckInit();
+
+ auto digitalPort =
+ hal::digitalChannelHandles->Get(outputPort, hal::HAL_HandleEnum::PWM);
+
+ if (!digitalPort) {
+ // If DIO was passed, channel error, else generic error
+ if (getHandleType(outputPort) == hal::HAL_HandleEnum::DIO) {
+ *status = HAL_LED_CHANNEL_ERROR;
+ } else {
+ *status = HAL_HANDLE_ERROR;
+ }
+ return HAL_kInvalidHandle;
+ }
+
+ if (digitalPort->channel >= kNumPWMHeaders) {
+ *status = HAL_LED_CHANNEL_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ auto handle = addressableLEDHandles->Allocate();
+
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+
+ auto led = addressableLEDHandles->Get(handle);
+
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ led->led.reset(tLED::create(status));
+
+ if (*status != 0) {
+ addressableLEDHandles->Free(handle);
+ return HAL_kInvalidHandle;
+ }
+
+ led->led->writeOutputSelect(digitalPort->channel, status);
+
+ if (*status != 0) {
+ addressableLEDHandles->Free(handle);
+ return HAL_kInvalidHandle;
+ }
+
+ led->ledBuffer = nullptr;
+ led->ledBufferSize = 0;
+
+ uint32_t session = led->led->getSystemInterface()->getHandle();
+
+ *status = OpenHostMemoryBuffer(session, "HMB_0_LED", &led->ledBuffer,
+ &led->ledBufferSize);
+
+ if (*status != 0) {
+ addressableLEDHandles->Free(handle);
+ return HAL_kInvalidHandle;
+ }
+
+ return handle;
+}
+
+void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle) {
+ addressableLEDHandles->Free(handle);
+}
+
+void HAL_SetAddressableLEDOutputPort(HAL_AddressableLEDHandle handle,
+ HAL_DigitalHandle outputPort,
+ int32_t* status) {
+ auto digitalPort =
+ hal::digitalChannelHandles->Get(outputPort, hal::HAL_HandleEnum::PWM);
+
+ if (!digitalPort) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ auto led = addressableLEDHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ led->led->writeOutputSelect(digitalPort->channel, status);
+}
+
+void HAL_SetAddressableLEDLength(HAL_AddressableLEDHandle handle,
+ int32_t length, int32_t* status) {
+ auto led = addressableLEDHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (length > HAL_kAddressableLEDMaxLength) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ led->led->strobeReset(status);
+
+ while (led->led->readPixelWriteIndex(status) != 0) {
+ }
+
+ if (*status != 0) {
+ return;
+ }
+
+ led->led->writeStringLength(length, status);
+
+ led->stringLength = length;
+}
+
+static_assert(sizeof(HAL_AddressableLEDData) == sizeof(uint32_t),
+ "LED Data must be 32 bit");
+
+void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle,
+ const struct HAL_AddressableLEDData* data,
+ int32_t length, int32_t* status) {
+ auto led = addressableLEDHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (length > led->stringLength) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ std::memcpy(led->ledBuffer, data, length * sizeof(HAL_AddressableLEDData));
+
+ asm("dmb");
+
+ led->led->strobeLoad(status);
+}
+
+void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle,
+ int32_t lowTime0NanoSeconds,
+ int32_t highTime0NanoSeconds,
+ int32_t lowTime1NanoSeconds,
+ int32_t highTime1NanoSeconds,
+ int32_t* status) {
+ auto led = addressableLEDHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ led->led->writeLowBitTickTiming(1, highTime0NanoSeconds / 25, status);
+ led->led->writeLowBitTickTiming(0, lowTime0NanoSeconds / 25, status);
+ led->led->writeHighBitTickTiming(1, highTime1NanoSeconds / 25, status);
+ led->led->writeHighBitTickTiming(0, lowTime1NanoSeconds / 25, status);
+}
+
+void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle,
+ int32_t syncTimeMicroSeconds,
+ int32_t* status) {
+ auto led = addressableLEDHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ led->led->writeSyncTiming(syncTimeMicroSeconds, status);
+}
+
+void HAL_StartAddressableLEDOutput(HAL_AddressableLEDHandle handle,
+ int32_t* status) {
+ auto led = addressableLEDHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ led->led->strobeStart(status);
+}
+
+void HAL_StopAddressableLEDOutput(HAL_AddressableLEDHandle handle,
+ int32_t* status) {
+ auto led = addressableLEDHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ led->led->strobeAbort(status);
+}
+} // extern "C"
diff --git a/hal/src/main/native/athena/AnalogGyro.cpp b/hal/src/main/native/athena/AnalogGyro.cpp
index 56b6f28..12d688d 100644
--- a/hal/src/main/native/athena/AnalogGyro.cpp
+++ b/hal/src/main/native/athena/AnalogGyro.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -181,7 +181,7 @@
if (*status != 0) return;
gyro->center = static_cast<int32_t>(
- static_cast<double>(value) / static_cast<double>(count) + .5);
+ static_cast<double>(value) / static_cast<double>(count) + 0.5);
gyro->offset = static_cast<double>(value) / static_cast<double>(count) -
static_cast<double>(gyro->center);
diff --git a/hal/src/main/native/athena/AnalogInput.cpp b/hal/src/main/native/athena/AnalogInput.cpp
index 859524e..1d502b9 100644
--- a/hal/src/main/native/athena/AnalogInput.cpp
+++ b/hal/src/main/native/athena/AnalogInput.cpp
@@ -201,6 +201,14 @@
return voltage;
}
+double HAL_GetAnalogValueToVolts(HAL_AnalogInputHandle analogPortHandle,
+ int32_t rawValue, int32_t* status) {
+ int32_t LSBWeight = HAL_GetAnalogLSBWeight(analogPortHandle, status);
+ int32_t offset = HAL_GetAnalogOffset(analogPortHandle, status);
+ double voltage = LSBWeight * 1.0e-9 * rawValue - offset * 1.0e-9;
+ return voltage;
+}
+
double HAL_GetAnalogAverageVoltage(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
int32_t value = HAL_GetAnalogAverageValue(analogPortHandle, status);
@@ -216,26 +224,38 @@
int32_t HAL_GetAnalogLSBWeight(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
- auto port = analogInputHandles->Get(analogPortHandle);
- if (port == nullptr) {
- *status = HAL_HANDLE_ERROR;
- return 0;
- }
- int32_t lsbWeight = FRC_NetworkCommunication_nAICalibration_getLSBWeight(
- 0, port->channel, status); // XXX: aiSystemIndex == 0?
- return lsbWeight;
+ // On the roboRIO, LSB is the same for all channels. So the channel lookup can
+ // be avoided
+ return FRC_NetworkCommunication_nAICalibration_getLSBWeight(0, 0, status);
+
+ // Keep the old code for future hardware
+
+ // auto port = analogInputHandles->Get(analogPortHandle);
+ // if (port == nullptr) {
+ // *status = HAL_HANDLE_ERROR;
+ // return 0;
+ // }
+ // int32_t lsbWeight = FRC_NetworkCommunication_nAICalibration_getLSBWeight(
+ // 0, port->channel, status); // XXX: aiSystemIndex == 0?
+ // return lsbWeight;
}
int32_t HAL_GetAnalogOffset(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
- auto port = analogInputHandles->Get(analogPortHandle);
- if (port == nullptr) {
- *status = HAL_HANDLE_ERROR;
- return 0;
- }
- int32_t offset = FRC_NetworkCommunication_nAICalibration_getOffset(
- 0, port->channel, status); // XXX: aiSystemIndex == 0?
- return offset;
+ // On the roboRIO, offset is the same for all channels. So the channel lookup
+ // can be avoided
+ return FRC_NetworkCommunication_nAICalibration_getOffset(0, 0, status);
+
+ // Keep the old code for future hardware
+
+ // auto port = analogInputHandles->Get(analogPortHandle);
+ // if (port == nullptr) {
+ // *status = HAL_HANDLE_ERROR;
+ // return 0;
+ // }
+ // int32_t offset = FRC_NetworkCommunication_nAICalibration_getOffset(
+ // 0, port->channel, status); // XXX: aiSystemIndex == 0?
+ // return offset;
}
} // extern "C"
diff --git a/hal/src/main/native/athena/AnalogTrigger.cpp b/hal/src/main/native/athena/AnalogTrigger.cpp
index 1841c51..9ec3f29 100644
--- a/hal/src/main/native/athena/AnalogTrigger.cpp
+++ b/hal/src/main/native/athena/AnalogTrigger.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -8,9 +8,11 @@
#include "hal/AnalogTrigger.h"
#include "AnalogInternal.h"
+#include "DutyCycleInternal.h"
#include "HALInitializer.h"
#include "PortsInternal.h"
#include "hal/AnalogInput.h"
+#include "hal/DutyCycle.h"
#include "hal/Errors.h"
#include "hal/handles/HandlesInternal.h"
#include "hal/handles/LimitedHandleResource.h"
@@ -21,7 +23,7 @@
struct AnalogTrigger {
std::unique_ptr<tAnalogTrigger> trigger;
- HAL_AnalogInputHandle analogHandle;
+ HAL_Handle handle;
uint8_t index;
};
@@ -46,7 +48,7 @@
extern "C" {
HAL_AnalogTriggerHandle HAL_InitializeAnalogTrigger(
- HAL_AnalogInputHandle portHandle, int32_t* index, int32_t* status) {
+ HAL_AnalogInputHandle portHandle, int32_t* status) {
hal::init::CheckInit();
// ensure we are given a valid and active AnalogInput handle
auto analog_port = analogInputHandles->Get(portHandle);
@@ -64,19 +66,46 @@
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
- trigger->analogHandle = portHandle;
+ trigger->handle = portHandle;
trigger->index = static_cast<uint8_t>(getHandleIndex(handle));
- *index = trigger->index;
trigger->trigger.reset(tAnalogTrigger::create(trigger->index, status));
trigger->trigger->writeSourceSelect_Channel(analog_port->channel, status);
return handle;
}
+HAL_AnalogTriggerHandle HAL_InitializeAnalogTriggerDutyCycle(
+ HAL_DutyCycleHandle dutyCycleHandle, int32_t* status) {
+ hal::init::CheckInit();
+ // ensure we are given a valid and active DutyCycle handle
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ if (dutyCycle == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ HAL_AnalogTriggerHandle handle = analogTriggerHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+ auto trigger = analogTriggerHandles->Get(handle);
+ if (trigger == nullptr) { // would only occur on thread issue
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ trigger->handle = dutyCycleHandle;
+ trigger->index = static_cast<uint8_t>(getHandleIndex(handle));
+
+ trigger->trigger.reset(tAnalogTrigger::create(trigger->index, status));
+ trigger->trigger->writeSourceSelect_Channel(dutyCycle->index, status);
+ trigger->trigger->writeSourceSelect_DutyCycle(true, status);
+ return handle;
+}
+
void HAL_CleanAnalogTrigger(HAL_AnalogTriggerHandle analogTriggerHandle,
int32_t* status) {
analogTriggerHandles->Free(analogTriggerHandle);
- // caller owns the analog input handle.
+ // caller owns the input handle.
}
void HAL_SetAnalogTriggerLimitsRaw(HAL_AnalogTriggerHandle analogTriggerHandle,
@@ -89,11 +118,46 @@
}
if (lower > upper) {
*status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR;
+ return;
}
trigger->trigger->writeLowerLimit(lower, status);
trigger->trigger->writeUpperLimit(upper, status);
}
+void HAL_SetAnalogTriggerLimitsDutyCycle(
+ HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
+ int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (getHandleType(trigger->handle) != HAL_HandleEnum::DutyCycle) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (lower > upper) {
+ *status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR;
+ return;
+ }
+
+ if (lower < 0.0 || upper > 1.0) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ int32_t scaleFactor =
+ HAL_GetDutyCycleOutputScaleFactor(trigger->handle, status);
+ if (*status != 0) {
+ return;
+ }
+
+ trigger->trigger->writeLowerLimit(static_cast<int32_t>(scaleFactor * lower),
+ status);
+ trigger->trigger->writeUpperLimit(static_cast<int32_t>(scaleFactor * upper),
+ status);
+}
+
void HAL_SetAnalogTriggerLimitsVoltage(
HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
int32_t* status) {
@@ -102,16 +166,22 @@
*status = HAL_HANDLE_ERROR;
return;
}
+
+ if (getHandleType(trigger->handle) != HAL_HandleEnum::AnalogInput) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
if (lower > upper) {
*status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR;
+ return;
}
// TODO: This depends on the averaged setting. Only raw values will work as
// is.
trigger->trigger->writeLowerLimit(
- HAL_GetAnalogVoltsToValue(trigger->analogHandle, lower, status), status);
+ HAL_GetAnalogVoltsToValue(trigger->handle, lower, status), status);
trigger->trigger->writeUpperLimit(
- HAL_GetAnalogVoltsToValue(trigger->analogHandle, upper, status), status);
+ HAL_GetAnalogVoltsToValue(trigger->handle, upper, status), status);
}
void HAL_SetAnalogTriggerAveraged(HAL_AnalogTriggerHandle analogTriggerHandle,
@@ -121,7 +191,8 @@
*status = HAL_HANDLE_ERROR;
return;
}
- if (trigger->trigger->readSourceSelect_Filter(status) != 0) {
+ if (trigger->trigger->readSourceSelect_Filter(status) != 0 ||
+ trigger->trigger->readSourceSelect_DutyCycle(status) != 0) {
*status = INCOMPATIBLE_STATE;
// TODO: wpi_setWPIErrorWithContext(IncompatibleMode, "Hardware does not
// support average and filtering at the same time.");
@@ -136,7 +207,8 @@
*status = HAL_HANDLE_ERROR;
return;
}
- if (trigger->trigger->readSourceSelect_Averaged(status) != 0) {
+ if (trigger->trigger->readSourceSelect_Averaged(status) != 0 ||
+ trigger->trigger->readSourceSelect_DutyCycle(status) != 0) {
*status = INCOMPATIBLE_STATE;
// TODO: wpi_setWPIErrorWithContext(IncompatibleMode, "Hardware does not "
// "support average and filtering at the same time.");
@@ -177,16 +249,27 @@
case HAL_Trigger_kInWindow:
result =
trigger->trigger->readOutput_InHysteresis(trigger->index, status);
- break; // XXX: Backport
+ break;
case HAL_Trigger_kState:
result = trigger->trigger->readOutput_OverLimit(trigger->index, status);
- break; // XXX: Backport
+ break;
case HAL_Trigger_kRisingPulse:
case HAL_Trigger_kFallingPulse:
*status = ANALOG_TRIGGER_PULSE_OUTPUT_ERROR;
return false;
+ break;
}
return result;
}
+int32_t HAL_GetAnalogTriggerFPGAIndex(
+ HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+ return trigger->index;
+}
+
} // extern "C"
diff --git a/hal/src/main/native/athena/DMA.cpp b/hal/src/main/native/athena/DMA.cpp
new file mode 100644
index 0000000..ee39d2d
--- /dev/null
+++ b/hal/src/main/native/athena/DMA.cpp
@@ -0,0 +1,1006 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/DMA.h"
+
+#include <array>
+#include <cstddef>
+#include <cstring>
+#include <memory>
+#include <type_traits>
+
+#include "AnalogInternal.h"
+#include "DigitalInternal.h"
+#include "EncoderInternal.h"
+#include "PortsInternal.h"
+#include "hal/AnalogAccumulator.h"
+#include "hal/AnalogGyro.h"
+#include "hal/AnalogInput.h"
+#include "hal/ChipObject.h"
+#include "hal/Errors.h"
+#include "hal/HALBase.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+
+using namespace hal;
+
+static_assert(std::is_standard_layout_v<HAL_DMASample>,
+ "HAL_DMASample must have standard layout");
+
+namespace {
+
+struct DMA {
+ std::unique_ptr<tDMAManager> manager;
+ std::unique_ptr<tDMA> aDMA;
+
+ HAL_DMASample captureStore;
+};
+} // namespace
+
+static constexpr size_t kChannelSize[22] = {2, 2, 4, 4, 2, 2, 4, 4, 3, 3, 2,
+ 1, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4};
+
+enum DMAOffsetConstants {
+ kEnable_AI0_Low = 0,
+ kEnable_AI0_High = 1,
+ kEnable_AIAveraged0_Low = 2,
+ kEnable_AIAveraged0_High = 3,
+ kEnable_AI1_Low = 4,
+ kEnable_AI1_High = 5,
+ kEnable_AIAveraged1_Low = 6,
+ kEnable_AIAveraged1_High = 7,
+ kEnable_Accumulator0 = 8,
+ kEnable_Accumulator1 = 9,
+ kEnable_DI = 10,
+ kEnable_AnalogTriggers = 11,
+ kEnable_Counters_Low = 12,
+ kEnable_Counters_High = 13,
+ kEnable_CounterTimers_Low = 14,
+ kEnable_CounterTimers_High = 15,
+ kEnable_Encoders_Low = 16,
+ kEnable_Encoders_High = 17,
+ kEnable_EncoderTimers_Low = 18,
+ kEnable_EncoderTimers_High = 19,
+ kEnable_DutyCycle_Low = 20,
+ kEnable_DutyCycle_High = 21,
+};
+
+static hal::LimitedHandleResource<HAL_DMAHandle, DMA, 1, HAL_HandleEnum::DMA>*
+ dmaHandles;
+
+namespace hal {
+namespace init {
+void InitializeDMA() {
+ static hal::LimitedHandleResource<HAL_DMAHandle, DMA, 1, HAL_HandleEnum::DMA>
+ dH;
+ dmaHandles = &dH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_DMAHandle HAL_InitializeDMA(int32_t* status) {
+ HAL_Handle handle = dmaHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+
+ auto dma = dmaHandles->Get(handle);
+
+ if (!dma) {
+ // Can only happen on thread error
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ // Manager does not get created until DMA is started
+ dma->aDMA.reset(tDMA::create(status));
+ if (*status != 0) {
+ dmaHandles->Free(handle);
+ return HAL_kInvalidHandle;
+ }
+
+ dma->aDMA->writeConfig_ExternalClock(false, status);
+ if (*status != 0) {
+ dmaHandles->Free(handle);
+ return HAL_kInvalidHandle;
+ }
+
+ HAL_SetDMARate(handle, 1, status);
+ if (*status != 0) {
+ dmaHandles->Free(handle);
+ return HAL_kInvalidHandle;
+ }
+
+ HAL_SetDMAPause(handle, false, status);
+ return handle;
+}
+
+void HAL_FreeDMA(HAL_DMAHandle handle) {
+ auto dma = dmaHandles->Get(handle);
+ dmaHandles->Free(handle);
+
+ if (!dma) return;
+
+ int32_t status = 0;
+ if (dma->manager) {
+ dma->manager->stop(&status);
+ }
+}
+
+void HAL_SetDMAPause(HAL_DMAHandle handle, HAL_Bool pause, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ dma->aDMA->writeConfig_Pause(pause, status);
+}
+void HAL_SetDMARate(HAL_DMAHandle handle, int32_t cycles, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (cycles < 1) {
+ cycles = 1;
+ }
+
+ dma->aDMA->writeRate(static_cast<uint32_t>(cycles), status);
+}
+
+void HAL_AddDMAEncoder(HAL_DMAHandle handle, HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ // Detect a counter encoder vs an actual encoder, and use the right DMA calls
+ HAL_FPGAEncoderHandle fpgaEncoderHandle = HAL_kInvalidHandle;
+ HAL_CounterHandle counterHandle = HAL_kInvalidHandle;
+
+ bool validEncoderHandle = hal::GetEncoderBaseHandle(
+ encoderHandle, &fpgaEncoderHandle, &counterHandle);
+
+ if (!validEncoderHandle) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (counterHandle != HAL_kInvalidHandle) {
+ HAL_AddDMACounter(handle, counterHandle, status);
+ return;
+ }
+
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (getHandleType(fpgaEncoderHandle) != HAL_HandleEnum::FPGAEncoder) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ int32_t index = getHandleIndex(fpgaEncoderHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (index < 4) {
+ dma->aDMA->writeConfig_Enable_Encoders_Low(true, status);
+ } else if (index < 8) {
+ dma->aDMA->writeConfig_Enable_Encoders_High(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_AddDMAEncoderPeriod(HAL_DMAHandle handle,
+ HAL_EncoderHandle encoderHandle, int32_t* status) {
+ // Detect a counter encoder vs an actual encoder, and use the right DMA calls
+ HAL_FPGAEncoderHandle fpgaEncoderHandle = HAL_kInvalidHandle;
+ HAL_CounterHandle counterHandle = HAL_kInvalidHandle;
+
+ bool validEncoderHandle = hal::GetEncoderBaseHandle(
+ encoderHandle, &fpgaEncoderHandle, &counterHandle);
+
+ if (!validEncoderHandle) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (counterHandle != HAL_kInvalidHandle) {
+ HAL_AddDMACounterPeriod(handle, counterHandle, status);
+ return;
+ }
+
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (getHandleType(fpgaEncoderHandle) != HAL_HandleEnum::FPGAEncoder) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ int32_t index = getHandleIndex(fpgaEncoderHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (index < 4) {
+ dma->aDMA->writeConfig_Enable_EncoderTimers_Low(true, status);
+ } else if (index < 8) {
+ dma->aDMA->writeConfig_Enable_EncoderTimers_High(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_AddDMACounter(HAL_DMAHandle handle, HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (getHandleType(counterHandle) != HAL_HandleEnum::Counter) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ int32_t index = getHandleIndex(counterHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (index < 4) {
+ dma->aDMA->writeConfig_Enable_Counters_Low(true, status);
+ } else if (index < 8) {
+ dma->aDMA->writeConfig_Enable_Counters_High(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_AddDMACounterPeriod(HAL_DMAHandle handle,
+ HAL_CounterHandle counterHandle, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (getHandleType(counterHandle) != HAL_HandleEnum::Counter) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ int32_t index = getHandleIndex(counterHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (index < 4) {
+ dma->aDMA->writeConfig_Enable_CounterTimers_Low(true, status);
+ } else if (index < 8) {
+ dma->aDMA->writeConfig_Enable_CounterTimers_High(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_AddDMADigitalSource(HAL_DMAHandle handle,
+ HAL_Handle digitalSourceHandle, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (isHandleType(digitalSourceHandle, HAL_HandleEnum::AnalogTrigger)) {
+ dma->aDMA->writeConfig_Enable_AnalogTriggers(true, status);
+ } else if (isHandleType(digitalSourceHandle, HAL_HandleEnum::DIO)) {
+ dma->aDMA->writeConfig_Enable_DI(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_AddDMAAnalogInput(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (getHandleType(aInHandle) != HAL_HandleEnum::AnalogInput) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ int32_t index = getHandleIndex(aInHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (index < 4) {
+ dma->aDMA->writeConfig_Enable_AI0_Low(true, status);
+ } else if (index < 8) {
+ dma->aDMA->writeConfig_Enable_AI0_High(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_AddDMADutyCycle(HAL_DMAHandle handle,
+ HAL_DutyCycleHandle dutyCycleHandle, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (getHandleType(dutyCycleHandle) != HAL_HandleEnum::DutyCycle) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ int32_t index = getHandleIndex(dutyCycleHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (index < 4) {
+ dma->aDMA->writeConfig_Enable_DutyCycle_Low(true, status);
+ } else if (index < 8) {
+ dma->aDMA->writeConfig_Enable_DutyCycle_High(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_AddDMAAveragedAnalogInput(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (getHandleType(aInHandle) != HAL_HandleEnum::AnalogInput) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ int32_t index = getHandleIndex(aInHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (index < 4) {
+ dma->aDMA->writeConfig_Enable_AIAveraged0_Low(true, status);
+ } else if (index < 8) {
+ dma->aDMA->writeConfig_Enable_AIAveraged0_High(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_AddDMAAnalogAccumulator(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (!HAL_IsAccumulatorChannel(aInHandle, status)) {
+ *status = HAL_INVALID_ACCUMULATOR_CHANNEL;
+ return;
+ }
+
+ int32_t index = getHandleIndex(aInHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (index == 0) {
+ dma->aDMA->writeConfig_Enable_Accumulator0(true, status);
+ } else if (index == 1) {
+ dma->aDMA->writeConfig_Enable_Accumulator1(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_SetDMAExternalTrigger(HAL_DMAHandle handle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_Bool rising, HAL_Bool falling,
+ int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ int index = 0;
+ auto triggerChannels = dma->captureStore.triggerChannels;
+ do {
+ if (((triggerChannels >> index) & 0x1) == 0) {
+ break;
+ }
+ index++;
+ } while (index < 8);
+
+ if (index == 8) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return;
+ }
+
+ dma->captureStore.triggerChannels |= (1 << index);
+
+ auto channelIndex = index;
+
+ auto isExternalClock = dma->aDMA->readConfig_ExternalClock(status);
+ if (*status == 0 && !isExternalClock) {
+ dma->aDMA->writeConfig_ExternalClock(true, status);
+ if (*status != 0) return;
+ } else if (*status != 0) {
+ return;
+ }
+
+ uint8_t pin = 0;
+ uint8_t module = 0;
+ bool analogTrigger = false;
+ bool success = remapDigitalSource(digitalSourceHandle, analogTriggerType, pin,
+ module, analogTrigger);
+
+ if (!success) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ tDMA::tExternalTriggers newTrigger;
+ newTrigger.FallingEdge = falling;
+ newTrigger.RisingEdge = rising;
+ newTrigger.ExternalClockSource_AnalogTrigger = analogTrigger;
+ newTrigger.ExternalClockSource_Channel = pin;
+ newTrigger.ExternalClockSource_Module = module;
+
+ dma->aDMA->writeExternalTriggers(channelIndex / 4, channelIndex % 4,
+ newTrigger, status);
+}
+
+void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = INCOMPATIBLE_STATE;
+ return;
+ }
+
+ tDMA::tConfig config = dma->aDMA->readConfig(status);
+ if (*status != 0) return;
+
+ {
+ size_t accum_size = 0;
+#define SET_SIZE(bit) \
+ if (config.bit) { \
+ dma->captureStore.channelOffsets[k##bit] = accum_size; \
+ accum_size += kChannelSize[k##bit]; \
+ } else { \
+ dma->captureStore.channelOffsets[k##bit] = -1; \
+ }
+ SET_SIZE(Enable_AI0_Low);
+ SET_SIZE(Enable_AI0_High);
+ SET_SIZE(Enable_AIAveraged0_Low);
+ SET_SIZE(Enable_AIAveraged0_High);
+ SET_SIZE(Enable_AI1_Low);
+ SET_SIZE(Enable_AI1_High);
+ SET_SIZE(Enable_AIAveraged1_Low);
+ SET_SIZE(Enable_AIAveraged1_High);
+ SET_SIZE(Enable_Accumulator0);
+ SET_SIZE(Enable_Accumulator1);
+ SET_SIZE(Enable_DI);
+ SET_SIZE(Enable_AnalogTriggers);
+ SET_SIZE(Enable_Counters_Low);
+ SET_SIZE(Enable_Counters_High);
+ SET_SIZE(Enable_CounterTimers_Low);
+ SET_SIZE(Enable_CounterTimers_High);
+ SET_SIZE(Enable_Encoders_Low);
+ SET_SIZE(Enable_Encoders_High);
+ SET_SIZE(Enable_EncoderTimers_Low);
+ SET_SIZE(Enable_EncoderTimers_High);
+ SET_SIZE(Enable_DutyCycle_Low);
+ SET_SIZE(Enable_DutyCycle_High);
+#undef SET_SIZE
+ dma->captureStore.captureSize = accum_size + 1;
+ }
+
+ dma->manager = std::make_unique<tDMAManager>(
+ g_DMA_index, queueDepth * dma->captureStore.captureSize, status);
+ if (*status != 0) {
+ return;
+ }
+
+ dma->manager->start(status);
+ dma->manager->stop(status);
+ dma->manager->start(status);
+}
+
+void HAL_StopDMA(HAL_DMAHandle handle, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ dma->manager->stop(status);
+ dma->manager = nullptr;
+ }
+}
+
+void* HAL_GetDMADirectPointer(HAL_DMAHandle handle) {
+ auto dma = dmaHandles->Get(handle);
+ return dma.get();
+}
+
+enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer,
+ HAL_DMASample* dmaSample,
+ int32_t timeoutMs,
+ int32_t* remainingOut,
+ int32_t* status) {
+ DMA* dma = static_cast<DMA*>(dmaPointer);
+ *remainingOut = 0;
+ size_t remainingBytes = 0;
+
+ if (!dma->manager) {
+ *status = INCOMPATIBLE_STATE;
+ return HAL_DMA_ERROR;
+ }
+
+ dma->manager->read(dmaSample->readBuffer, dma->captureStore.captureSize,
+ timeoutMs, &remainingBytes, status);
+
+ *remainingOut = remainingBytes / dma->captureStore.captureSize;
+
+ if (*status == 0) {
+ uint32_t lower_sample =
+ dmaSample->readBuffer[dma->captureStore.captureSize - 1];
+ dmaSample->timeStamp = HAL_ExpandFPGATime(lower_sample, status);
+ if (*status != 0) {
+ return HAL_DMA_ERROR;
+ }
+ dmaSample->triggerChannels = dma->captureStore.triggerChannels;
+ dmaSample->captureSize = dma->captureStore.captureSize;
+ std::memcpy(dmaSample->channelOffsets, dma->captureStore.channelOffsets,
+ sizeof(dmaSample->channelOffsets));
+ return HAL_DMA_OK;
+ } else if (*status == NiFpga_Status_FifoTimeout) {
+ *status = 0;
+ return HAL_DMA_TIMEOUT;
+ } else {
+ return HAL_DMA_ERROR;
+ }
+}
+
+enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle,
+ HAL_DMASample* dmaSample, int32_t timeoutMs,
+ int32_t* remainingOut, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_DMA_ERROR;
+ }
+
+ return HAL_ReadDMADirect(dma.get(), dmaSample, timeoutMs, remainingOut,
+ status);
+}
+
+static uint32_t ReadDMAValue(const HAL_DMASample& dma, int valueType, int index,
+ int32_t* status) {
+ auto offset = dma.channelOffsets[valueType];
+ if (offset == -1) {
+ *status = NiFpga_Status_ResourceNotFound;
+ return 0;
+ }
+ return dma.readBuffer[offset + index];
+}
+
+uint64_t HAL_GetDMASampleTime(const HAL_DMASample* dmaSample, int32_t* status) {
+ return dmaSample->timeStamp;
+}
+
+int32_t HAL_GetDMASampleEncoderRaw(const HAL_DMASample* dmaSample,
+ HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ HAL_FPGAEncoderHandle fpgaEncoderHandle = 0;
+ HAL_CounterHandle counterHandle = 0;
+ bool validEncoderHandle = hal::GetEncoderBaseHandle(
+ encoderHandle, &fpgaEncoderHandle, &counterHandle);
+
+ if (!validEncoderHandle) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ if (counterHandle != HAL_kInvalidHandle) {
+ return HAL_GetDMASampleCounter(dmaSample, counterHandle, status);
+ }
+
+ if (getHandleType(fpgaEncoderHandle) != HAL_HandleEnum::FPGAEncoder) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ int32_t index = getHandleIndex(fpgaEncoderHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ uint32_t dmaWord = 0;
+ *status = 0;
+ if (index < 4) {
+ dmaWord = ReadDMAValue(*dmaSample, kEnable_Encoders_Low, index, status);
+ } else if (index < 8) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_Encoders_High, index - 4, status);
+ } else {
+ *status = NiFpga_Status_ResourceNotFound;
+ }
+ if (*status != 0) {
+ return -1;
+ }
+
+ return static_cast<int32_t>(dmaWord) >> 1;
+}
+
+int32_t HAL_GetDMASampleEncoderPeriodRaw(const HAL_DMASample* dmaSample,
+ HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ HAL_FPGAEncoderHandle fpgaEncoderHandle = 0;
+ HAL_CounterHandle counterHandle = 0;
+ bool validEncoderHandle = hal::GetEncoderBaseHandle(
+ encoderHandle, &fpgaEncoderHandle, &counterHandle);
+
+ if (!validEncoderHandle) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ if (counterHandle != HAL_kInvalidHandle) {
+ return HAL_GetDMASampleCounterPeriod(dmaSample, counterHandle, status);
+ }
+
+ if (getHandleType(fpgaEncoderHandle) != HAL_HandleEnum::FPGAEncoder) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ int32_t index = getHandleIndex(fpgaEncoderHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ uint32_t dmaWord = 0;
+ *status = 0;
+ if (index < 4) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_EncoderTimers_Low, index, status);
+ } else if (index < 8) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_EncoderTimers_High, index - 4, status);
+ } else {
+ *status = NiFpga_Status_ResourceNotFound;
+ }
+ if (*status != 0) {
+ return -1;
+ }
+
+ return static_cast<int32_t>(dmaWord) & 0x7FFFFF;
+}
+
+int32_t HAL_GetDMASampleCounter(const HAL_DMASample* dmaSample,
+ HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ if (getHandleType(counterHandle) != HAL_HandleEnum::Counter) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ int32_t index = getHandleIndex(counterHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ uint32_t dmaWord = 0;
+ *status = 0;
+ if (index < 4) {
+ dmaWord = ReadDMAValue(*dmaSample, kEnable_Counters_Low, index, status);
+ } else if (index < 8) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_Counters_High, index - 4, status);
+ } else {
+ *status = NiFpga_Status_ResourceNotFound;
+ }
+ if (*status != 0) {
+ return -1;
+ }
+
+ return static_cast<int32_t>(dmaWord) >> 1;
+}
+
+int32_t HAL_GetDMASampleCounterPeriod(const HAL_DMASample* dmaSample,
+ HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ if (getHandleType(counterHandle) != HAL_HandleEnum::Counter) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ int32_t index = getHandleIndex(counterHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ uint32_t dmaWord = 0;
+ *status = 0;
+ if (index < 4) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_CounterTimers_Low, index, status);
+ } else if (index < 8) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_CounterTimers_High, index - 4, status);
+ } else {
+ *status = NiFpga_Status_ResourceNotFound;
+ }
+ if (*status != 0) {
+ return -1;
+ }
+
+ return static_cast<int32_t>(dmaWord) & 0x7FFFFF;
+}
+
+HAL_Bool HAL_GetDMASampleDigitalSource(const HAL_DMASample* dmaSample,
+ HAL_Handle dSourceHandle,
+ int32_t* status) {
+ HAL_HandleEnum handleType = getHandleType(dSourceHandle);
+ int32_t index = getHandleIndex(dSourceHandle);
+
+ *status = 0;
+ if (handleType == HAL_HandleEnum::DIO) {
+ auto readVal = ReadDMAValue(*dmaSample, kEnable_DI, 0, status);
+ if (*status == 0) {
+ if (index < kNumDigitalHeaders) {
+ return (readVal >> index) & 0x1;
+ } else {
+ return (readVal >> (index + 6)) & 0x1;
+ }
+ }
+ } else if (handleType == HAL_HandleEnum::AnalogTrigger) {
+ auto readVal = ReadDMAValue(*dmaSample, kEnable_AnalogTriggers, 0, status);
+ if (*status == 0) {
+ return (readVal >> index) & 0x1;
+ }
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+ return false;
+}
+int32_t HAL_GetDMASampleAnalogInputRaw(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status) {
+ if (getHandleType(aInHandle) != HAL_HandleEnum::AnalogInput) {
+ *status = HAL_HANDLE_ERROR;
+ return 0xFFFFFFFF;
+ }
+
+ int32_t index = getHandleIndex(aInHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return 0xFFFFFFFF;
+ }
+
+ uint32_t dmaWord = 0;
+ if (index < 4) {
+ dmaWord = ReadDMAValue(*dmaSample, kEnable_AI0_Low, index / 2, status);
+ } else if (index < 8) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_AI0_High, (index - 4) / 2, status);
+ } else {
+ *status = NiFpga_Status_ResourceNotFound;
+ }
+ if (*status != 0) {
+ return 0xFFFFFFFF;
+ }
+
+ if (index % 2) {
+ return (dmaWord >> 16) & 0xffff;
+ } else {
+ return dmaWord & 0xffff;
+ }
+}
+
+int32_t HAL_GetDMASampleAveragedAnalogInputRaw(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status) {
+ if (getHandleType(aInHandle) != HAL_HandleEnum::AnalogInput) {
+ *status = HAL_HANDLE_ERROR;
+ return 0xFFFFFFFF;
+ }
+
+ int32_t index = getHandleIndex(aInHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return 0xFFFFFFFF;
+ }
+
+ uint32_t dmaWord = 0;
+ if (index < 4) {
+ dmaWord = ReadDMAValue(*dmaSample, kEnable_AIAveraged0_Low, index, status);
+ } else if (index < 8) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_AIAveraged0_High, index - 4, status);
+ } else {
+ *status = NiFpga_Status_ResourceNotFound;
+ }
+ if (*status != 0) {
+ return 0xFFFFFFFF;
+ }
+
+ return dmaWord;
+}
+
+void HAL_GetDMASampleAnalogAccumulator(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int64_t* count, int64_t* value,
+ int32_t* status) {
+ if (!HAL_IsAccumulatorChannel(aInHandle, status)) {
+ *status = HAL_INVALID_ACCUMULATOR_CHANNEL;
+ return;
+ }
+
+ int32_t index = getHandleIndex(aInHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ uint32_t dmaWord = 0;
+ uint32_t dmaValue1 = 0;
+ uint32_t dmaValue2 = 0;
+ if (index == 0) {
+ dmaWord = ReadDMAValue(*dmaSample, kEnable_Accumulator0, index, status);
+ dmaValue1 =
+ ReadDMAValue(*dmaSample, kEnable_Accumulator0, index + 1, status);
+ dmaValue2 =
+ ReadDMAValue(*dmaSample, kEnable_Accumulator0, index + 2, status);
+ } else if (index == 1) {
+ dmaWord = ReadDMAValue(*dmaSample, kEnable_Accumulator1, index - 1, status);
+ dmaValue1 = ReadDMAValue(*dmaSample, kEnable_Accumulator0, index, status);
+ dmaValue2 =
+ ReadDMAValue(*dmaSample, kEnable_Accumulator0, index + 1, status);
+ } else {
+ *status = NiFpga_Status_ResourceNotFound;
+ }
+ if (*status != 0) {
+ return;
+ }
+
+ *count = dmaWord;
+
+ *value = static_cast<int64_t>(dmaValue1) << 32 | dmaValue2;
+}
+
+int32_t HAL_GetDMASampleDutyCycleOutputRaw(const HAL_DMASample* dmaSample,
+ HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ if (getHandleType(dutyCycleHandle) != HAL_HandleEnum::DutyCycle) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ int32_t index = getHandleIndex(dutyCycleHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ uint32_t dmaWord = 0;
+ *status = 0;
+ if (index < 4) {
+ dmaWord = ReadDMAValue(*dmaSample, kEnable_DutyCycle_Low, index, status);
+ } else if (index < 8) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_DutyCycle_High, index - 4, status);
+ } else {
+ *status = NiFpga_Status_ResourceNotFound;
+ }
+ if (*status != 0) {
+ return -1;
+ }
+ return dmaWord;
+}
+} // extern "C"
diff --git a/hal/src/main/native/athena/DigitalInternal.cpp b/hal/src/main/native/athena/DigitalInternal.cpp
index 684d35a..205ce3e 100644
--- a/hal/src/main/native/athena/DigitalInternal.cpp
+++ b/hal/src/main/native/athena/DigitalInternal.cpp
@@ -103,9 +103,9 @@
(kSystemClockTicksPerMicrosecond * 1e3);
pwmSystem->writeConfig_Period(
- static_cast<uint16_t>(kDefaultPwmPeriod / loopTime + .5), status);
+ static_cast<uint16_t>(kDefaultPwmPeriod / loopTime + 0.5), status);
uint16_t minHigh = static_cast<uint16_t>(
- (kDefaultPwmCenter - kDefaultPwmStepsDown * loopTime) / loopTime + .5);
+ (kDefaultPwmCenter - kDefaultPwmStepsDown * loopTime) / loopTime + 0.5);
pwmSystem->writeConfig_MinHigh(minHigh, status);
// Ensure that PWM output values are set to OFF
for (uint8_t pwmIndex = 0; pwmIndex < kNumPWMChannels; pwmIndex++) {
diff --git a/hal/src/main/native/athena/DigitalInternal.h b/hal/src/main/native/athena/DigitalInternal.h
index b486f48..2cb9b3c 100644
--- a/hal/src/main/native/athena/DigitalInternal.h
+++ b/hal/src/main/native/athena/DigitalInternal.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -97,6 +97,20 @@
uint8_t& channel, uint8_t& module, bool& analogTrigger);
/**
+ * Remap the Digital Channel to map to the bitfield channel of the FPGA
+ */
+constexpr int32_t remapDigitalChannelToBitfieldChannel(int32_t channel) {
+ // First 10 are headers
+ if (channel < kNumDigitalHeaders) return channel;
+ // 2nd group of 16 are mxp. So if mxp port, add 6, since they start at 10
+ else if (channel < kNumDigitalMXPChannels)
+ return channel + 6;
+ // Assume SPI, so remove MXP channels
+ else
+ return channel - kNumDigitalMXPChannels;
+}
+
+/**
* Map DIO channel numbers from their physical number (10 to 26) to their
* position in the bit field.
*/
diff --git a/hal/src/main/native/athena/DutyCycle.cpp b/hal/src/main/native/athena/DutyCycle.cpp
new file mode 100644
index 0000000..1c1a678
--- /dev/null
+++ b/hal/src/main/native/athena/DutyCycle.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/DutyCycle.h"
+
+#include <memory>
+
+#include "DigitalInternal.h"
+#include "DutyCycleInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/ChipObject.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+
+using namespace hal;
+
+namespace hal {
+LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
+ HAL_HandleEnum::DutyCycle>* dutyCycleHandles;
+namespace init {
+void InitializeDutyCycle() {
+ static LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
+ HAL_HandleEnum::DutyCycle>
+ dcH;
+ dutyCycleHandles = &dcH;
+}
+} // namespace init
+} // namespace hal
+
+static constexpr int32_t kScaleFactor = 4e7 - 1;
+
+extern "C" {
+HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType triggerType,
+ int32_t* status) {
+ hal::init::CheckInit();
+
+ bool routingAnalogTrigger = false;
+ uint8_t routingChannel = 0;
+ uint8_t routingModule = 0;
+ bool success =
+ remapDigitalSource(digitalSourceHandle, triggerType, routingChannel,
+ routingModule, routingAnalogTrigger);
+
+ if (!success) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ HAL_DutyCycleHandle handle = dutyCycleHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+
+ auto dutyCycle = dutyCycleHandles->Get(handle);
+ uint32_t index = static_cast<uint32_t>(getHandleIndex(handle));
+ dutyCycle->dutyCycle.reset(tDutyCycle::create(index, status));
+
+ dutyCycle->dutyCycle->writeSource_AnalogTrigger(routingAnalogTrigger, status);
+ dutyCycle->dutyCycle->writeSource_Channel(routingChannel, status);
+ dutyCycle->dutyCycle->writeSource_Module(routingModule, status);
+ dutyCycle->index = index;
+
+ return handle;
+}
+void HAL_FreeDutyCycle(HAL_DutyCycleHandle dutyCycleHandle) {
+ // Just free it, the unique ptr will take care of everything else
+ dutyCycleHandles->Free(dutyCycleHandle);
+}
+
+int32_t HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ if (!dutyCycle) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ // TODO Handle Overflow
+ unsigned char overflow = 0;
+ return dutyCycle->dutyCycle->readFrequency(&overflow, status);
+}
+
+double HAL_GetDutyCycleOutput(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ return HAL_GetDutyCycleOutputRaw(dutyCycleHandle, status) /
+ static_cast<double>(kScaleFactor);
+}
+
+int32_t HAL_GetDutyCycleOutputRaw(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ if (!dutyCycle) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ // TODO Handle Overflow
+ unsigned char overflow = 0;
+ return dutyCycle->dutyCycle->readOutput(&overflow, status);
+}
+
+int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ return kScaleFactor;
+}
+
+int32_t HAL_GetDutyCycleFPGAIndex(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ if (!dutyCycle) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+ return dutyCycle->index;
+}
+} // extern "C"
diff --git a/hal/src/main/native/athena/DutyCycleInternal.h b/hal/src/main/native/athena/DutyCycleInternal.h
new file mode 100644
index 0000000..33a8ff2
--- /dev/null
+++ b/hal/src/main/native/athena/DutyCycleInternal.h
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "hal/ChipObject.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+
+namespace hal {
+struct DutyCycle {
+ std::unique_ptr<tDutyCycle> dutyCycle;
+ int index;
+};
+
+extern LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
+ HAL_HandleEnum::DutyCycle>* dutyCycleHandles;
+} // namespace hal
diff --git a/hal/src/main/native/athena/Encoder.cpp b/hal/src/main/native/athena/Encoder.cpp
index bf3a273..be8c203 100644
--- a/hal/src/main/native/athena/Encoder.cpp
+++ b/hal/src/main/native/athena/Encoder.cpp
@@ -35,7 +35,7 @@
return;
}
m_counter = HAL_kInvalidHandle;
- SetMaxPeriod(.5, status);
+ SetMaxPeriod(0.5, status);
break;
}
case HAL_Encoder_k1X:
@@ -238,6 +238,19 @@
} // namespace init
} // namespace hal
+namespace hal {
+bool GetEncoderBaseHandle(HAL_EncoderHandle handle,
+ HAL_FPGAEncoderHandle* fpgaHandle,
+ HAL_CounterHandle* counterHandle) {
+ auto encoder = encoderHandles->Get(handle);
+ if (!handle) return false;
+
+ *fpgaHandle = encoder->m_encoder;
+ *counterHandle = encoder->m_counter;
+ return true;
+}
+} // namespace hal
+
extern "C" {
HAL_EncoderHandle HAL_InitializeEncoder(
HAL_Handle digitalSourceHandleA, HAL_AnalogTriggerType analogTriggerTypeA,
diff --git a/hal/src/main/native/athena/EncoderInternal.h b/hal/src/main/native/athena/EncoderInternal.h
index 1e7ed4d..bed4ee3 100644
--- a/hal/src/main/native/athena/EncoderInternal.h
+++ b/hal/src/main/native/athena/EncoderInternal.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -13,8 +13,16 @@
namespace hal {
+bool GetEncoderBaseHandle(HAL_EncoderHandle handle,
+ HAL_FPGAEncoderHandle* fpgaEncoderHandle,
+ HAL_CounterHandle* counterHandle);
+
class Encoder {
public:
+ friend bool GetEncoderBaseHandle(HAL_EncoderHandle handle,
+ HAL_FPGAEncoderHandle* fpgaEncoderHandle,
+ HAL_CounterHandle* counterHandle);
+
Encoder(HAL_Handle digitalSourceHandleA,
HAL_AnalogTriggerType analogTriggerTypeA,
HAL_Handle digitalSourceHandleB,
diff --git a/hal/src/main/native/athena/FRCDriverStation.cpp b/hal/src/main/native/athena/FRCDriverStation.cpp
index 23c874f..5b29815 100644
--- a/hal/src/main/native/athena/FRCDriverStation.cpp
+++ b/hal/src/main/native/athena/FRCDriverStation.cpp
@@ -124,7 +124,7 @@
static wpi::mutex* newDSDataAvailableMutex;
static wpi::condition_variable* newDSDataAvailableCond;
-static int newDSDataAvailableCounter{0};
+static std::atomic_int newDSDataAvailableCounter{0};
namespace hal {
namespace init {
@@ -338,14 +338,46 @@
FRC_NetworkCommunication_observeUserProgramTest();
}
-HAL_Bool HAL_IsNewControlData(void) {
+static int& GetThreadLocalLastCount() {
// There is a rollover error condition here. At Packet# = n * (uintmax), this
// will return false when instead it should return true. However, this at a
// 20ms rate occurs once every 2.7 years of DS connected runtime, so not
// worth the cycles to check.
thread_local int lastCount{-1};
- std::lock_guard lock{*newDSDataAvailableMutex};
- int currentCount = newDSDataAvailableCounter;
+ return lastCount;
+}
+
+void HAL_WaitForCachedControlData(void) {
+ HAL_WaitForCachedControlDataTimeout(0);
+}
+
+HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout) {
+ int& lastCount = GetThreadLocalLastCount();
+ int currentCount = newDSDataAvailableCounter.load();
+ if (lastCount != currentCount) {
+ lastCount = currentCount;
+ return true;
+ }
+ auto timeoutTime =
+ std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+
+ std::unique_lock lock{*newDSDataAvailableMutex};
+ while (newDSDataAvailableCounter.load() == currentCount) {
+ if (timeout > 0) {
+ auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime);
+ if (timedOut == std::cv_status::timeout) {
+ return false;
+ }
+ } else {
+ newDSDataAvailableCond->wait(lock);
+ }
+ }
+ return true;
+}
+
+HAL_Bool HAL_IsNewControlData(void) {
+ int& lastCount = GetThreadLocalLastCount();
+ int currentCount = newDSDataAvailableCounter.load();
if (lastCount == currentCount) return false;
lastCount = currentCount;
return true;
@@ -365,9 +397,9 @@
auto timeoutTime =
std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+ int currentCount = newDSDataAvailableCounter.load();
std::unique_lock lock{*newDSDataAvailableMutex};
- int currentCount = newDSDataAvailableCounter;
- while (newDSDataAvailableCounter == currentCount) {
+ while (newDSDataAvailableCounter.load() == currentCount) {
if (timeout > 0) {
auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime);
if (timedOut == std::cv_status::timeout) {
@@ -388,7 +420,7 @@
// to signal our threads
if (refNum != refNumber) return;
// Notify all threads
- newDSDataAvailableCounter++;
+ newDSDataAvailableCounter.fetch_add(1);
newDSDataAvailableCond->notify_all();
}
diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp
index a9abde6..91214e5 100644
--- a/hal/src/main/native/athena/HAL.cpp
+++ b/hal/src/main/native/athena/HAL.cpp
@@ -42,6 +42,7 @@
namespace hal {
namespace init {
void InitializeHAL() {
+ InitializeAddressableLED();
InitializeAccelerometer();
InitializeAnalogAccumulator();
InitializeAnalogGyro();
@@ -56,6 +57,8 @@
InitializeCounter();
InitializeDigitalInternal();
InitializeDIO();
+ InitializeDMA();
+ InitializeDutyCycle();
InitializeEncoder();
InitializeFPGAEncoder();
InitializeFRCDriverStation();
@@ -212,6 +215,10 @@
return ERR_FRCSystem_NetCommNotResponding_MESSAGE;
case ERR_FRCSystem_NoDSConnection:
return ERR_FRCSystem_NoDSConnection_MESSAGE;
+ case HAL_CAN_BUFFER_OVERRUN:
+ return HAL_CAN_BUFFER_OVERRUN_MESSAGE;
+ case HAL_LED_CHANNEL_ERROR:
+ return HAL_LED_CHANNEL_ERROR_MESSAGE;
default:
return "Unknown error status";
}
@@ -253,6 +260,28 @@
return (upper2 << 32) + lower;
}
+uint64_t HAL_ExpandFPGATime(uint32_t unexpanded_lower, int32_t* status) {
+ // Capture the current FPGA time. This will give us the upper half of the
+ // clock.
+ uint64_t fpga_time = HAL_GetFPGATime(status);
+ if (*status != 0) return 0;
+
+ // Now, we need to detect the case where the lower bits rolled over after we
+ // sampled. In that case, the upper bits will be 1 bigger than they should
+ // be.
+
+ // Break it into lower and upper portions.
+ uint32_t lower = fpga_time & 0xffffffffull;
+ uint64_t upper = (fpga_time >> 32) & 0xffffffff;
+
+ // The time was sampled *before* the current time, so roll it back.
+ if (lower < unexpanded_lower) {
+ --upper;
+ }
+
+ return (upper << 32) + static_cast<uint64_t>(unexpanded_lower);
+}
+
HAL_Bool HAL_GetFPGAButton(int32_t* status) {
if (!global) {
*status = NiFpga_Status_ResourceNotInitialized;
diff --git a/hal/src/main/native/athena/HALInitializer.h b/hal/src/main/native/athena/HALInitializer.h
index fc38038..acce386 100644
--- a/hal/src/main/native/athena/HALInitializer.h
+++ b/hal/src/main/native/athena/HALInitializer.h
@@ -19,6 +19,7 @@
}
extern void InitializeAccelerometer();
+extern void InitializeAddressableLED();
extern void InitializeAnalogAccumulator();
extern void InitializeAnalogGyro();
extern void InitializeAnalogInput();
@@ -32,6 +33,8 @@
extern void InitializeCounter();
extern void InitializeDigitalInternal();
extern void InitializeDIO();
+extern void InitializeDMA();
+extern void InitializeDutyCycle();
extern void InitializeEncoder();
extern void InitializeFPGAEncoder();
extern void InitializeFRCDriverStation();
diff --git a/hal/src/main/native/athena/Interrupts.cpp b/hal/src/main/native/athena/Interrupts.cpp
index b0b2071..78d518c 100644
--- a/hal/src/main/native/athena/Interrupts.cpp
+++ b/hal/src/main/native/athena/Interrupts.cpp
@@ -64,12 +64,6 @@
}
};
-} // namespace
-
-static void threadedInterruptHandler(uint32_t mask, void* param) {
- static_cast<InterruptThreadOwner*>(param)->Notify(mask);
-}
-
struct Interrupt {
std::unique_ptr<tInterrupt> anInterrupt;
std::unique_ptr<tInterruptManager> manager;
@@ -77,6 +71,12 @@
void* param = nullptr;
};
+} // namespace
+
+static void threadedInterruptHandler(uint32_t mask, void* param) {
+ static_cast<InterruptThreadOwner*>(param)->Notify(mask);
+}
+
static LimitedHandleResource<HAL_InterruptHandle, Interrupt, kNumInterrupts,
HAL_HandleEnum::Interrupt>* interruptHandles;
@@ -118,7 +118,11 @@
if (anInterrupt == nullptr) {
return nullptr;
}
- anInterrupt->manager->disable(status);
+
+ if (anInterrupt->manager->isEnabled(status)) {
+ anInterrupt->manager->disable(status);
+ }
+
void* param = anInterrupt->param;
return param;
}
@@ -152,7 +156,10 @@
*status = HAL_HANDLE_ERROR;
return;
}
- anInterrupt->manager->enable(status);
+
+ if (!anInterrupt->manager->isEnabled(status)) {
+ anInterrupt->manager->enable(status);
+ }
}
void HAL_DisableInterrupts(HAL_InterruptHandle interruptHandle,
@@ -162,7 +169,9 @@
*status = HAL_HANDLE_ERROR;
return;
}
- anInterrupt->manager->disable(status);
+ if (anInterrupt->manager->isEnabled(status)) {
+ anInterrupt->manager->disable(status);
+ }
}
int64_t HAL_ReadInterruptRisingTimestamp(HAL_InterruptHandle interruptHandle,
diff --git a/hal/src/main/native/athena/Notifier.cpp b/hal/src/main/native/athena/Notifier.cpp
index 662b04e..c30e8d1 100644
--- a/hal/src/main/native/athena/Notifier.cpp
+++ b/hal/src/main/native/athena/Notifier.cpp
@@ -139,6 +139,9 @@
return handle;
}
+void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
+ int32_t* status) {}
+
void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
if (!notifier) return;
diff --git a/hal/src/main/native/athena/PDP.cpp b/hal/src/main/native/athena/PDP.cpp
index f27f5da..f5cf92b 100644
--- a/hal/src/main/native/athena/PDP.cpp
+++ b/hal/src/main/native/athena/PDP.cpp
@@ -175,7 +175,11 @@
HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
- return pdpStatus.bits.temp * 1.03250836957542 - 67.8564500484966;
+ if (*status != 0) {
+ return 0;
+ } else {
+ return pdpStatus.bits.temp * 1.03250836957542 - 67.8564500484966;
+ }
}
double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status) {
@@ -186,7 +190,11 @@
HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
- return pdpStatus.bits.busVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
+ if (*status != 0) {
+ return 0;
+ } else {
+ return pdpStatus.bits.busVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
+ }
}
double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
@@ -205,6 +213,9 @@
PdpStatus1 pdpStatus;
HAL_ReadCANPacketTimeout(handle, Status1, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) {
+ return 0;
+ }
switch (channel) {
case 0:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
@@ -235,6 +246,9 @@
PdpStatus2 pdpStatus;
HAL_ReadCANPacketTimeout(handle, Status2, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) {
+ return 0;
+ }
switch (channel) {
case 6:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan7_h8) << 2) |
@@ -265,6 +279,9 @@
PdpStatus3 pdpStatus;
HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) {
+ return 0;
+ }
switch (channel) {
case 12:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan13_h8) << 2) |
@@ -289,6 +306,75 @@
return raw * 0.125; /* 7.3 fixed pt value in Amps */
}
+void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents,
+ int32_t* status) {
+ int32_t length = 0;
+ uint64_t receivedTimestamp = 0;
+ PdpStatus1 pdpStatus;
+ HAL_ReadCANPacketTimeout(handle, Status1, pdpStatus.data, &length,
+ &receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) return;
+ PdpStatus2 pdpStatus2;
+ HAL_ReadCANPacketTimeout(handle, Status2, pdpStatus2.data, &length,
+ &receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) return;
+ PdpStatus3 pdpStatus3;
+ HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus3.data, &length,
+ &receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) return;
+
+ currents[0] = ((static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
+ pdpStatus.bits.chan1_l2) *
+ 0.125;
+ currents[1] = ((static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
+ pdpStatus.bits.chan2_l4) *
+ 0.125;
+ currents[2] = ((static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
+ pdpStatus.bits.chan3_l6) *
+ 0.125;
+ currents[3] = ((static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
+ pdpStatus.bits.chan4_l8) *
+ 0.125;
+ currents[4] = ((static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
+ pdpStatus.bits.chan5_l2) *
+ 0.125;
+ currents[5] = ((static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
+ pdpStatus.bits.chan6_l4) *
+ 0.125;
+
+ currents[6] = ((static_cast<uint32_t>(pdpStatus2.bits.chan7_h8) << 2) |
+ pdpStatus2.bits.chan7_l2) *
+ 0.125;
+ currents[7] = ((static_cast<uint32_t>(pdpStatus2.bits.chan8_h6) << 4) |
+ pdpStatus2.bits.chan8_l4) *
+ 0.125;
+ currents[8] = ((static_cast<uint32_t>(pdpStatus2.bits.chan9_h4) << 6) |
+ pdpStatus2.bits.chan9_l6) *
+ 0.125;
+ currents[9] = ((static_cast<uint32_t>(pdpStatus2.bits.chan10_h2) << 8) |
+ pdpStatus2.bits.chan10_l8) *
+ 0.125;
+ currents[10] = ((static_cast<uint32_t>(pdpStatus2.bits.chan11_h8) << 2) |
+ pdpStatus2.bits.chan11_l2) *
+ 0.125;
+ currents[11] = ((static_cast<uint32_t>(pdpStatus2.bits.chan12_h6) << 4) |
+ pdpStatus2.bits.chan12_l4) *
+ 0.125;
+
+ currents[12] = ((static_cast<uint32_t>(pdpStatus3.bits.chan13_h8) << 2) |
+ pdpStatus3.bits.chan13_l2) *
+ 0.125;
+ currents[13] = ((static_cast<uint32_t>(pdpStatus3.bits.chan14_h6) << 4) |
+ pdpStatus3.bits.chan14_l4) *
+ 0.125;
+ currents[14] = ((static_cast<uint32_t>(pdpStatus3.bits.chan15_h4) << 6) |
+ pdpStatus3.bits.chan15_l6) *
+ 0.125;
+ currents[15] = ((static_cast<uint32_t>(pdpStatus3.bits.chan16_h2) << 8) |
+ pdpStatus3.bits.chan16_l8) *
+ 0.125;
+}
+
double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) {
PdpStatusEnergy pdpStatus;
int32_t length = 0;
@@ -296,6 +382,9 @@
HAL_ReadCANPacketTimeout(handle, StatusEnergy, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) {
+ return 0;
+ }
uint32_t raw;
raw = pdpStatus.bits.TotalCurrent_125mAperunit_h8;
@@ -311,6 +400,9 @@
HAL_ReadCANPacketTimeout(handle, StatusEnergy, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) {
+ return 0;
+ }
uint32_t raw;
raw = pdpStatus.bits.Power_125mWperunit_h4;
@@ -328,6 +420,9 @@
HAL_ReadCANPacketTimeout(handle, StatusEnergy, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) {
+ return 0;
+ }
uint32_t raw;
raw = pdpStatus.bits.Energy_125mWPerUnitXTmeas_h4;
diff --git a/hal/src/main/native/athena/Ports.cpp b/hal/src/main/native/athena/Ports.cpp
index 9a52736..47bd400 100644
--- a/hal/src/main/native/athena/Ports.cpp
+++ b/hal/src/main/native/athena/Ports.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -37,5 +37,7 @@
int32_t HAL_GetNumSolenoidChannels(void) { return kNumSolenoidChannels; }
int32_t HAL_GetNumPDPModules(void) { return kNumPDPModules; }
int32_t HAL_GetNumPDPChannels(void) { return kNumPDPChannels; }
+int32_t HAL_GetNumDutyCycles(void) { return kNumDutyCycles; }
+int32_t HAL_GetNumAddressableLEDs(void) { return kNumAddressableLEDs; }
} // extern "C"
diff --git a/hal/src/main/native/athena/PortsInternal.h b/hal/src/main/native/athena/PortsInternal.h
index b3eb6b0..98fc690 100644
--- a/hal/src/main/native/athena/PortsInternal.h
+++ b/hal/src/main/native/athena/PortsInternal.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -35,5 +35,7 @@
constexpr int32_t kNumSolenoidChannels = 8;
constexpr int32_t kNumPDPModules = 63;
constexpr int32_t kNumPDPChannels = 16;
+constexpr int32_t kNumDutyCycles = tDutyCycle::kNumSystems;
+constexpr int32_t kNumAddressableLEDs = tLED::kNumSystems;
} // namespace hal
diff --git a/hal/src/main/native/athena/SPI.cpp b/hal/src/main/native/athena/SPI.cpp
index bb0666a..80cbf09 100644
--- a/hal/src/main/native/athena/SPI.cpp
+++ b/hal/src/main/native/athena/SPI.cpp
@@ -565,7 +565,7 @@
void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend,
int32_t dataSize, int32_t zeroSize,
int32_t* status) {
- if (dataSize < 0 || dataSize > 16) {
+ if (dataSize < 0 || dataSize > 32) {
*status = PARAMETER_OUT_OF_RANGE;
return;
}
@@ -589,7 +589,7 @@
// set byte counts
tSPI::tAutoByteCount config;
config.ZeroByteCount = static_cast<unsigned>(zeroSize) & 0x7f;
- config.TxByteCount = static_cast<unsigned>(dataSize) & 0xf;
+ config.TxByteCount = static_cast<unsigned>(dataSize) & 0x1f;
spiSystem->writeAutoByteCount(config, status);
}
@@ -631,4 +631,12 @@
return spiSystem->readTransferSkippedFullCount(status);
}
+// These 2 functions are so the new stall functionality
+// can be tested. How they're used is not very clear
+// but I want them to be testable so we can add an impl.
+// We will not be including these in the headers
+void* HAL_GetSPIDMAManager() { return spiAutoDMA.get(); }
+
+void* HAL_GetSPISystem() { return spiSystem.get(); }
+
} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp b/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp
new file mode 100644
index 0000000..3f76e6a
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp
@@ -0,0 +1,145 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_AddressableLEDJNI.h"
+#include "hal/AddressableLED.h"
+
+using namespace frc;
+using namespace wpi::java;
+
+static_assert(sizeof(jbyte) * 4 == sizeof(HAL_AddressableLEDData));
+
+extern "C" {
+/*
+ * Class: edu_wpi_first_hal_AddressableLEDJNI
+ * Method: initialize
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AddressableLEDJNI_initialize
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ auto ret = HAL_InitializeAddressableLED(
+ static_cast<HAL_DigitalHandle>(handle), &status);
+ CheckStatus(env, status);
+ return ret;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AddressableLEDJNI
+ * Method: free
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AddressableLEDJNI_free
+ (JNIEnv* env, jclass, jint handle)
+{
+ HAL_FreeAddressableLED(static_cast<HAL_AddressableLEDHandle>(handle));
+}
+
+/*
+ * Class: edu_wpi_first_hal_AddressableLEDJNI
+ * Method: setLength
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AddressableLEDJNI_setLength
+ (JNIEnv* env, jclass, jint handle, jint length)
+{
+ int32_t status = 0;
+ HAL_SetAddressableLEDLength(static_cast<HAL_AddressableLEDHandle>(handle),
+ length, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AddressableLEDJNI
+ * Method: setData
+ * Signature: (I[B)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AddressableLEDJNI_setData
+ (JNIEnv* env, jclass, jint handle, jbyteArray arr)
+{
+ int32_t status = 0;
+ JByteArrayRef jArrRef{env, arr};
+ auto arrRef = jArrRef.array();
+ HAL_WriteAddressableLEDData(
+ static_cast<HAL_AddressableLEDHandle>(handle),
+ reinterpret_cast<const HAL_AddressableLEDData*>(arrRef.data()),
+ arrRef.size() / 4, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AddressableLEDJNI
+ * Method: setBitTiming
+ * Signature: (IIIII)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AddressableLEDJNI_setBitTiming
+ (JNIEnv* env, jclass, jint handle, jint lowTime0, jint highTime0,
+ jint lowTime1, jint highTime1)
+{
+ int32_t status = 0;
+ HAL_SetAddressableLEDBitTiming(static_cast<HAL_AddressableLEDHandle>(handle),
+ lowTime0, highTime0, lowTime1, highTime1,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AddressableLEDJNI
+ * Method: setSyncTime
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AddressableLEDJNI_setSyncTime
+ (JNIEnv* env, jclass, jint handle, jint syncTime)
+{
+ int32_t status = 0;
+ HAL_SetAddressableLEDSyncTime(static_cast<HAL_AddressableLEDHandle>(handle),
+ syncTime, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AddressableLEDJNI
+ * Method: start
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AddressableLEDJNI_start
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ HAL_StartAddressableLEDOutput(static_cast<HAL_AddressableLEDHandle>(handle),
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AddressableLEDJNI
+ * Method: stop
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AddressableLEDJNI_stop
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ HAL_StopAddressableLEDOutput(static_cast<HAL_AddressableLEDHandle>(handle),
+ &status);
+ CheckStatus(env, status);
+}
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/AnalogJNI.cpp b/hal/src/main/native/cpp/jni/AnalogJNI.cpp
index abd614f..5571d55 100644
--- a/hal/src/main/native/cpp/jni/AnalogJNI.cpp
+++ b/hal/src/main/native/cpp/jni/AnalogJNI.cpp
@@ -486,18 +486,31 @@
/*
* Class: edu_wpi_first_hal_AnalogJNI
* Method: initializeAnalogTrigger
- * Signature: (ILjava/lang/Object;)I
+ * Signature: (I)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_AnalogJNI_initializeAnalogTrigger
- (JNIEnv* env, jclass, jint id, jobject index)
+ (JNIEnv* env, jclass, jint id)
{
- jint* indexHandle =
- reinterpret_cast<jint*>(env->GetDirectBufferAddress(index));
int32_t status = 0;
- HAL_AnalogTriggerHandle analogTrigger = HAL_InitializeAnalogTrigger(
- (HAL_AnalogInputHandle)id, reinterpret_cast<int32_t*>(indexHandle),
- &status);
+ HAL_AnalogTriggerHandle analogTrigger =
+ HAL_InitializeAnalogTrigger((HAL_AnalogInputHandle)id, &status);
+ CheckStatus(env, status);
+ return (jint)analogTrigger;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: initializeAnalogTriggerDutyCycle
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_initializeAnalogTriggerDutyCycle
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_AnalogTriggerHandle analogTrigger =
+ HAL_InitializeAnalogTriggerDutyCycle((HAL_DutyCycleHandle)id, &status);
CheckStatus(env, status);
return (jint)analogTrigger;
}
@@ -533,6 +546,21 @@
/*
* Class: edu_wpi_first_hal_AnalogJNI
+ * Method: setAnalogTriggerLimitsDutyCycle
+ * Signature: (IDD)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_setAnalogTriggerLimitsDutyCycle
+ (JNIEnv* env, jclass, jint id, jdouble lower, jdouble upper)
+{
+ int32_t status = 0;
+ HAL_SetAnalogTriggerLimitsDutyCycle((HAL_AnalogTriggerHandle)id, lower, upper,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
* Method: setAnalogTriggerLimitsVoltage
* Signature: (IDD)V
*/
@@ -622,4 +650,20 @@
return val;
}
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAnalogTriggerFPGAIndex
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogTriggerFPGAIndex
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ auto val =
+ HAL_GetAnalogTriggerFPGAIndex((HAL_AnalogTriggerHandle)id, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/DigitalGlitchFilterJNI.cpp b/hal/src/main/native/cpp/jni/DigitalGlitchFilterJNI.cpp
index 3e39ac0..fbbaadb 100644
--- a/hal/src/main/native/cpp/jni/DigitalGlitchFilterJNI.cpp
+++ b/hal/src/main/native/cpp/jni/DigitalGlitchFilterJNI.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -13,6 +13,8 @@
using namespace frc;
+extern "C" {
+
/*
* Class: edu_wpi_first_hal_DigitalGlitchFilterJNI
* Method: setFilterSelect
@@ -76,3 +78,5 @@
CheckStatus(env, status);
return result;
}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/DutyCycleJNI.cpp b/hal/src/main/native/cpp/jni/DutyCycleJNI.cpp
new file mode 100644
index 0000000..510ca00
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/DutyCycleJNI.cpp
@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_DutyCycleJNI.h"
+#include "hal/DutyCycle.h"
+
+using namespace frc;
+
+extern "C" {
+/*
+ * Class: edu_wpi_first_hal_DutyCycleJNI
+ * Method: initialize
+ * Signature: (II)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_DutyCycleJNI_initialize
+ (JNIEnv* env, jclass, jint digitalSourceHandle, jint analogTriggerType)
+{
+ int32_t status = 0;
+ auto handle = HAL_InitializeDutyCycle(
+ static_cast<HAL_Handle>(digitalSourceHandle),
+ static_cast<HAL_AnalogTriggerType>(analogTriggerType), &status);
+ CheckStatus(env, status);
+ return handle;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DutyCycleJNI
+ * Method: free
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DutyCycleJNI_free
+ (JNIEnv*, jclass, jint handle)
+{
+ HAL_FreeDutyCycle(static_cast<HAL_DutyCycleHandle>(handle));
+}
+
+/*
+ * Class: edu_wpi_first_hal_DutyCycleJNI
+ * Method: getFrequency
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_DutyCycleJNI_getFrequency
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleFrequency(
+ static_cast<HAL_DutyCycleHandle>(handle), &status);
+ CheckStatus(env, status);
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DutyCycleJNI
+ * Method: getOutput
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_DutyCycleJNI_getOutput
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ auto retVal =
+ HAL_GetDutyCycleOutput(static_cast<HAL_DutyCycleHandle>(handle), &status);
+ CheckStatus(env, status);
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DutyCycleJNI
+ * Method: getOutputRaw
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_DutyCycleJNI_getOutputRaw
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleOutputRaw(
+ static_cast<HAL_DutyCycleHandle>(handle), &status);
+ CheckStatus(env, status);
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DutyCycleJNI
+ * Method: getOutputScaleFactor
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_DutyCycleJNI_getOutputScaleFactor
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleOutputScaleFactor(
+ static_cast<HAL_DutyCycleHandle>(handle), &status);
+ CheckStatus(env, status);
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DutyCycleJNI
+ * Method: getFPGAIndex
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_DutyCycleJNI_getFPGAIndex
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleFPGAIndex(
+ static_cast<HAL_DutyCycleHandle>(handle), &status);
+ CheckStatus(env, status);
+ return retVal;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/NotifierJNI.cpp b/hal/src/main/native/cpp/jni/NotifierJNI.cpp
index 588614e..874d750 100644
--- a/hal/src/main/native/cpp/jni/NotifierJNI.cpp
+++ b/hal/src/main/native/cpp/jni/NotifierJNI.cpp
@@ -10,6 +10,8 @@
#include <cassert>
#include <cstdio>
+#include <wpi/jni_util.h>
+
#include "HALUtil.h"
#include "edu_wpi_first_hal_NotifierJNI.h"
#include "hal/Notifier.h"
@@ -39,6 +41,21 @@
/*
* Class: edu_wpi_first_hal_NotifierJNI
+ * Method: setNotifierName
+ * Signature: (ILjava/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_NotifierJNI_setNotifierName
+ (JNIEnv* env, jclass cls, jint notifierHandle, jstring name)
+{
+ int32_t status = 0;
+ HAL_SetNotifierName((HAL_NotifierHandle)notifierHandle,
+ wpi::java::JStringRef{env, name}.c_str(), &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_NotifierJNI
* Method: stopNotifier
* Signature: (I)V
*/
diff --git a/hal/src/main/native/cpp/jni/PDPJNI.cpp b/hal/src/main/native/cpp/jni/PDPJNI.cpp
index e8173be..f649a8d 100644
--- a/hal/src/main/native/cpp/jni/PDPJNI.cpp
+++ b/hal/src/main/native/cpp/jni/PDPJNI.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -100,6 +100,25 @@
/*
* Class: edu_wpi_first_hal_PDPJNI
+ * Method: getPDPAllCurrents
+ * Signature: (I[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PDPJNI_getPDPAllCurrents
+ (JNIEnv* env, jclass, jint handle, jdoubleArray jarr)
+{
+ double storage[16];
+ int32_t status = 0;
+ HAL_GetPDPAllChannelCurrents(handle, storage, &status);
+ if (!CheckStatus(env, status, false)) {
+ return;
+ }
+
+ env->SetDoubleArrayRegion(jarr, 0, 16, storage);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PDPJNI
* Method: getPDPTotalCurrent
* Signature: (I)D
*/
diff --git a/hal/src/main/native/include/hal/AddressableLED.h b/hal/src/main/native/include/hal/AddressableLED.h
new file mode 100644
index 0000000..68383ef
--- /dev/null
+++ b/hal/src/main/native/include/hal/AddressableLED.h
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <stdint.h>
+
+#include "hal/AddressableLEDTypes.h"
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_addressable Addressable LED Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+HAL_AddressableLEDHandle HAL_InitializeAddressableLED(
+ HAL_DigitalHandle outputPort, int32_t* status);
+
+void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle);
+
+void HAL_SetAddressableLEDOutputPort(HAL_AddressableLEDHandle handle,
+ HAL_DigitalHandle outputPort,
+ int32_t* status);
+
+void HAL_SetAddressableLEDLength(HAL_AddressableLEDHandle handle,
+ int32_t length, int32_t* status);
+
+void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle,
+ const struct HAL_AddressableLEDData* data,
+ int32_t length, int32_t* status);
+
+void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle,
+ int32_t lowTime0NanoSeconds,
+ int32_t highTime0NanoSeconds,
+ int32_t lowTime1NanoSeconds,
+ int32_t highTime1NanoSeconds,
+ int32_t* status);
+
+void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle,
+ int32_t syncTimeMicroSeconds,
+ int32_t* status);
+
+void HAL_StartAddressableLEDOutput(HAL_AddressableLEDHandle handle,
+ int32_t* status);
+
+void HAL_StopAddressableLEDOutput(HAL_AddressableLEDHandle handle,
+ int32_t* status);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp b/hal/src/main/native/include/hal/AddressableLEDTypes.h
similarity index 64%
copy from wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
copy to hal/src/main/native/include/hal/AddressableLEDTypes.h
index 8bd62ea..bdcd742 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
+++ b/hal/src/main/native/include/hal/AddressableLEDTypes.h
@@ -5,12 +5,15 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include "frc2/command/PrintCommand.h"
+#pragma once
-using namespace frc2;
+#include <stdint.h>
-PrintCommand::PrintCommand(const wpi::Twine& message)
- : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
-}
+#define HAL_kAddressableLEDMaxLength 5460
-bool PrintCommand::RunsWhenDisabled() const { return true; }
+struct HAL_AddressableLEDData {
+ uint8_t b;
+ uint8_t g;
+ uint8_t r;
+ uint8_t padding;
+};
diff --git a/hal/src/main/native/include/hal/AnalogInput.h b/hal/src/main/native/include/hal/AnalogInput.h
index c0e45b3..95ba4e0 100644
--- a/hal/src/main/native/include/hal/AnalogInput.h
+++ b/hal/src/main/native/include/hal/AnalogInput.h
@@ -233,6 +233,16 @@
*/
int32_t HAL_GetAnalogOffset(HAL_AnalogInputHandle analogPortHandle,
int32_t* status);
+
+/**
+ * Get the analog voltage from a raw value.
+ *
+ * @param analogPortHandle Handle to the analog port the values were read from.
+ * @param rawValue The raw analog value
+ * @return The voltage relating to the value
+ */
+double HAL_GetAnalogValueToVolts(HAL_AnalogInputHandle analogPortHandle,
+ int32_t rawValue, int32_t* status);
#ifdef __cplusplus
} // extern "C"
#endif
diff --git a/hal/src/main/native/include/hal/AnalogTrigger.h b/hal/src/main/native/include/hal/AnalogTrigger.h
index f461f1d..85d7680 100644
--- a/hal/src/main/native/include/hal/AnalogTrigger.h
+++ b/hal/src/main/native/include/hal/AnalogTrigger.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -37,11 +37,17 @@
* Initializes an analog trigger.
*
* @param portHandle the analog input to use for triggering
- * @param index the trigger index value (output)
* @return the created analog trigger handle
*/
HAL_AnalogTriggerHandle HAL_InitializeAnalogTrigger(
- HAL_AnalogInputHandle portHandle, int32_t* index, int32_t* status);
+ HAL_AnalogInputHandle portHandle, int32_t* status);
+
+/**
+ * Initializes an analog trigger with a Duty Cycle input
+ *
+ */
+HAL_AnalogTriggerHandle HAL_InitializeAnalogTriggerDutyCycle(
+ HAL_DutyCycleHandle dutyCycleHandle, int32_t* status);
/**
* Frees an analog trigger.
@@ -54,7 +60,8 @@
/**
* Sets the raw ADC upper and lower limits of the analog trigger.
*
- * HAL_SetAnalogTriggerLimitsVoltage is likely better in most cases.
+ * HAL_SetAnalogTriggerLimitsVoltage or HAL_SetAnalogTriggerLimitsDutyCycle
+ * is likely better in most cases.
*
* @param analogTriggerHandle the trigger handle
* @param lower the lower ADC value
@@ -77,12 +84,19 @@
HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
int32_t* status);
+void HAL_SetAnalogTriggerLimitsDutyCycle(
+ HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
+ int32_t* status);
+
/**
* Configures 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.
*
+ * This is not allowed to be used if filtered mode is set.
+ * This is not allowed to be used with Duty Cycle based inputs.
+ *
* @param analogTriggerHandle the trigger handle
* @param useAveragedValue true to use averaged values, false for raw
*/
@@ -96,6 +110,8 @@
* is designed to help with 360 degree pot applications for the period where the
* pot crosses through zero.
*
+ * This is not allowed to be used if averaged mode is set.
+ *
* @param analogTriggerHandle the trigger handle
* @param useFilteredValue true to use filtered values, false for average or
* raw
@@ -137,6 +153,15 @@
HAL_Bool HAL_GetAnalogTriggerOutput(HAL_AnalogTriggerHandle analogTriggerHandle,
HAL_AnalogTriggerType type,
int32_t* status);
+
+/**
+ * Get the FPGA index for the AnlogTrigger.
+ *
+ * @param analogTriggerHandle the trigger handle
+ * @return the FPGA index
+ */
+int32_t HAL_GetAnalogTriggerFPGAIndex(
+ HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status);
#ifdef __cplusplus
} // extern "C"
#endif
diff --git a/hal/src/main/native/include/hal/CANAPITypes.h b/hal/src/main/native/include/hal/CANAPITypes.h
index 31a64ab..a23cee1 100644
--- a/hal/src/main/native/include/hal/CANAPITypes.h
+++ b/hal/src/main/native/include/hal/CANAPITypes.h
@@ -55,7 +55,8 @@
HAL_CAN_Man_kTeamUse = 8,
HAL_CAN_Man_kKauaiLabs = 9,
HAL_CAN_Man_kCopperforge = 10,
- HAL_CAN_Man_kPWF = 11
+ HAL_CAN_Man_kPWF = 11,
+ HAL_CAN_Man_kStudica = 12
};
// clang-format on
/** @} */
diff --git a/hal/src/main/native/include/hal/ChipObject.h b/hal/src/main/native/include/hal/ChipObject.h
index 878595b..9b321c2 100644
--- a/hal/src/main/native/include/hal/ChipObject.h
+++ b/hal/src/main/native/include/hal/ChipObject.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -24,9 +24,11 @@
#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tCounter.h>
#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tDIO.h>
#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tDMA.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tDutyCycle.h>
#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tEncoder.h>
#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tGlobal.h>
#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tInterrupt.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tLED.h>
#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tPWM.h>
#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tPower.h>
#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tRelay.h>
diff --git a/hal/src/main/native/include/hal/DMA.h b/hal/src/main/native/include/hal/DMA.h
new file mode 100644
index 0000000..38aaca2
--- /dev/null
+++ b/hal/src/main/native/include/hal/DMA.h
@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <stdint.h>
+
+#include "hal/AnalogTrigger.h"
+#include "hal/Types.h"
+
+// clang-format off
+/**
+ * The DMA Read Status.
+ */
+HAL_ENUM(HAL_DMAReadStatus ) {
+ HAL_DMA_OK = 1,
+ HAL_DMA_TIMEOUT = 2,
+ HAL_DMA_ERROR = 3,
+};
+// clang-format on
+
+struct HAL_DMASample {
+ uint32_t readBuffer[74];
+ int32_t channelOffsets[22];
+ uint64_t timeStamp;
+ uint32_t captureSize;
+ uint8_t triggerChannels;
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+HAL_DMAHandle HAL_InitializeDMA(int32_t* status);
+void HAL_FreeDMA(HAL_DMAHandle handle);
+
+void HAL_SetDMAPause(HAL_DMAHandle handle, HAL_Bool pause, int32_t* status);
+void HAL_SetDMARate(HAL_DMAHandle handle, int32_t cycles, int32_t* status);
+
+void HAL_AddDMAEncoder(HAL_DMAHandle handle, HAL_EncoderHandle encoderHandle,
+ int32_t* status);
+void HAL_AddDMAEncoderPeriod(HAL_DMAHandle handle,
+ HAL_EncoderHandle encoderHandle, int32_t* status);
+void HAL_AddDMACounter(HAL_DMAHandle handle, HAL_CounterHandle counterHandle,
+ int32_t* status);
+void HAL_AddDMACounterPeriod(HAL_DMAHandle handle,
+ HAL_CounterHandle counterHandle, int32_t* status);
+void HAL_AddDMADigitalSource(HAL_DMAHandle handle,
+ HAL_Handle digitalSourceHandle, int32_t* status);
+void HAL_AddDMAAnalogInput(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle, int32_t* status);
+
+void HAL_AddDMAAveragedAnalogInput(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status);
+
+void HAL_AddDMAAnalogAccumulator(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status);
+
+void HAL_AddDMADutyCycle(HAL_DMAHandle handle,
+ HAL_DutyCycleHandle dutyCycleHandle, int32_t* status);
+
+void HAL_SetDMAExternalTrigger(HAL_DMAHandle handle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_Bool rising, HAL_Bool falling,
+ int32_t* status);
+
+void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status);
+void HAL_StopDMA(HAL_DMAHandle handle, int32_t* status);
+
+void* HAL_GetDMADirectPointer(HAL_DMAHandle handle);
+
+enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer,
+ HAL_DMASample* dmaSample,
+ int32_t timeoutMs,
+ int32_t* remainingOut,
+ int32_t* status);
+
+enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle,
+ HAL_DMASample* dmaSample, int32_t timeoutMs,
+ int32_t* remainingOut, int32_t* status);
+
+// Sampling Code
+uint64_t HAL_GetDMASampleTime(const HAL_DMASample* dmaSample, int32_t* status);
+
+int32_t HAL_GetDMASampleEncoderRaw(const HAL_DMASample* dmaSample,
+ HAL_EncoderHandle encoderHandle,
+ int32_t* status);
+
+int32_t HAL_GetDMASampleCounter(const HAL_DMASample* dmaSample,
+ HAL_CounterHandle counterHandle,
+ int32_t* status);
+
+int32_t HAL_GetDMASampleEncoderPeriodRaw(const HAL_DMASample* dmaSample,
+ HAL_EncoderHandle encoderHandle,
+ int32_t* status);
+
+int32_t HAL_GetDMASampleCounterPeriod(const HAL_DMASample* dmaSample,
+ HAL_CounterHandle counterHandle,
+ int32_t* status);
+HAL_Bool HAL_GetDMASampleDigitalSource(const HAL_DMASample* dmaSample,
+ HAL_Handle dSourceHandle,
+ int32_t* status);
+int32_t HAL_GetDMASampleAnalogInputRaw(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status);
+
+int32_t HAL_GetDMASampleAveragedAnalogInputRaw(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status);
+
+void HAL_GetDMASampleAnalogAccumulator(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int64_t* count, int64_t* value,
+ int32_t* status);
+
+int32_t HAL_GetDMASampleDutyCycleOutputRaw(const HAL_DMASample* dmaSample,
+ HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/hal/DriverStation.h b/hal/src/main/native/include/hal/DriverStation.h
index db98698..471c18b 100644
--- a/hal/src/main/native/include/hal/DriverStation.h
+++ b/hal/src/main/native/include/hal/DriverStation.h
@@ -194,6 +194,24 @@
void HAL_ReleaseDSMutex(void);
/**
+ * Checks if new control data has arrived since the last
+ * HAL_WaitForCachedControlData or HAL_IsNewControlData call. If new data has
+ * not arrived, waits for new data to arrive. Otherwise, returns immediately.
+ */
+void HAL_WaitForCachedControlData(void);
+
+/**
+ * Checks if new control data has arrived since the last
+ * HAL_WaitForCachedControlData or HAL_IsNewControlData call. If new data has
+ * not arrived, waits for new data to arrive, or a timeout. Otherwise, returns
+ * immediately.
+ *
+ * @param timeout timeout in seconds
+ * @return true for new data, false for timeout
+ */
+HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout);
+
+/**
* Has a new control packet from the driver station arrived since the last
* time this function was called?
*
diff --git a/hal/src/main/native/include/hal/DutyCycle.h b/hal/src/main/native/include/hal/DutyCycle.h
new file mode 100644
index 0000000..357d8f3
--- /dev/null
+++ b/hal/src/main/native/include/hal/DutyCycle.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/AnalogTrigger.h"
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_dutycycle DutyCycle Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initialize a DutyCycle input.
+ *
+ * @param digitalSourceHandle the digital source to use (either a
+ * HAL_DigitalHandle or a HAL_AnalogTriggerHandle)
+ * @param triggerType the analog trigger type of the source if it is
+ * an analog trigger
+ * @return thre created duty cycle handle
+ */
+HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType triggerType,
+ int32_t* status);
+
+/**
+ * Free a DutyCycle.
+ *
+ * @param dutyCycleHandle the duty cycle handle
+ */
+void HAL_FreeDutyCycle(HAL_DutyCycleHandle dutyCycleHandle);
+
+/**
+ * Indicates the duty cycle is used by a simulated device.
+ *
+ * @param handle the duty cycle handle
+ * @param device simulated device handle
+ */
+void HAL_SetDutyCycleSimDevice(HAL_DutyCycleHandle handle,
+ HAL_SimDeviceHandle device);
+
+/**
+ * Get the frequency of the duty cycle signal.
+ *
+ * @param dutyCycleHandle the duty cycle handle
+ * @return frequency in Hertz
+ */
+int32_t HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status);
+
+/**
+ * Get the output ratio of the duty cycle signal.
+ *
+ * <p> 0 means always low, 1 means always high.
+ *
+ * @param dutyCycleHandle the duty cycle handle
+ * @return output ratio between 0 and 1
+ */
+double HAL_GetDutyCycleOutput(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status);
+
+/**
+ * Get the raw output ratio of the duty cycle signal.
+ *
+ * <p> 0 means always low, an output equal to
+ * GetOutputScaleFactor() means always high.
+ *
+ * @param dutyCycleHandle the duty cycle handle
+ * @return output ratio in raw units
+ */
+int32_t HAL_GetDutyCycleOutputRaw(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status);
+
+/**
+ * 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.
+ *
+ * @param dutyCycleHandle the duty cycle handle
+ * @return the output scale factor
+ */
+int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status);
+
+/**
+ * Get the FPGA index for the DutyCycle.
+ *
+ * @param dutyCycleHandle the duty cycle handle
+ * @return the FPGA index
+ */
+int32_t HAL_GetDutyCycleFPGAIndex(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/Errors.h b/hal/src/main/native/include/hal/Errors.h
index 4476ab0..9f74f8c 100644
--- a/hal/src/main/native/include/hal/Errors.h
+++ b/hal/src/main/native/include/hal/Errors.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -94,6 +94,14 @@
#define HAL_HANDLE_ERROR_MESSAGE \
"HAL: A handle parameter was passed incorrectly"
+#define HAL_LED_CHANNEL_ERROR -1099
+#define HAL_LED_CHANNEL_ERROR_MESSAGE \
+ "HAL: Addressable LEDs only supported on PWM Headers, not MXP or DIO"
+
+#define HAL_INVALID_DMA_ADDITION -1102
+#define HAL_INVALID_DMA_ADDITION_MESSAGE \
+ "HAL_AddDMA() only works before HAL_StartDMA()"
+
#define HAL_SERIAL_PORT_NOT_FOUND -1123
#define HAL_SERIAL_PORT_NOT_FOUND_MESSAGE \
"HAL: The specified serial port device was not found"
@@ -117,6 +125,13 @@
#define HAL_CAN_TIMEOUT -1154
#define HAL_CAN_TIMEOUT_MESSAGE "HAL: CAN Receive has Timed Out"
+#define HAL_SIM_NOT_SUPPORTED -1155
+#define HAL_SIM_NOT_SUPPORTED_MESSAGE "HAL: Method not supported in sim"
+
+#define HAL_CAN_BUFFER_OVERRUN -35007
+#define HAL_CAN_BUFFER_OVERRUN_MESSAGE \
+ "HAL: CAN Output Buffer Full. Ensure a device is attached"
+
#define VI_ERROR_SYSTEM_ERROR_MESSAGE "HAL - VISA: System Error";
#define VI_ERROR_INV_OBJECT_MESSAGE "HAL - VISA: Invalid Object"
#define VI_ERROR_RSRC_LOCKED_MESSAGE "HAL - VISA: Resource Locked"
diff --git a/hal/src/main/native/include/hal/Extensions.h b/hal/src/main/native/include/hal/Extensions.h
index 0fcbcba..3a435c0 100644
--- a/hal/src/main/native/include/hal/Extensions.h
+++ b/hal/src/main/native/include/hal/Extensions.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -7,6 +7,8 @@
#pragma once
+#include "hal/Types.h"
+
/**
* @defgroup hal_extensions Simulator Extensions
* @ingroup hal_capi
@@ -41,5 +43,19 @@
* @return the succes state of the initialization
*/
int HAL_LoadExtensions(void);
+
+/**
+ * Enables or disables the message saying no HAL extensions were found.
+ *
+ * Some apps don't care, and the message create clutter. For general team code,
+ * we want it.
+ *
+ * This must be called before HAL_Initialize is called.
+ *
+ * This defaults to true.
+ *
+ * @param showMessage true to show message, false to not.
+ */
+void HAL_SetShowExtensionsNotFoundMessages(HAL_Bool showMessage);
} // extern "C"
/** @} */
diff --git a/hal/src/main/native/include/hal/HAL.h b/hal/src/main/native/include/hal/HAL.h
index 2f71d79..4412886 100644
--- a/hal/src/main/native/include/hal/HAL.h
+++ b/hal/src/main/native/include/hal/HAL.h
@@ -24,6 +24,7 @@
#include "hal/DriverStation.h"
#include "hal/Encoder.h"
#include "hal/Errors.h"
+#include "hal/FRCUsageReporting.h"
#include "hal/HALBase.h"
#include "hal/I2C.h"
#include "hal/Interrupts.h"
@@ -41,7 +42,3 @@
#include "hal/Threads.h"
#include "hal/Types.h"
#include "hal/Value.h"
-
-#ifdef __cplusplus
-#include "hal/FRCUsageReporting.h"
-#endif
diff --git a/hal/src/main/native/include/hal/HALBase.h b/hal/src/main/native/include/hal/HALBase.h
index a958f2b..f61d9e4 100644
--- a/hal/src/main/native/include/hal/HALBase.h
+++ b/hal/src/main/native/include/hal/HALBase.h
@@ -110,6 +110,20 @@
uint64_t HAL_GetFPGATime(int32_t* status);
/**
+ * Given an 32 bit FPGA time, expand it to the nearest likely 64 bit FPGA time.
+ *
+ * Note: This is making the assumption that the timestamp being converted is
+ * always in the past. If you call this with a future timestamp, it probably
+ * will make it in the past. If you wait over 70 minutes between capturing the
+ * bottom 32 bits of the timestamp and expanding it, you will be off by
+ * multiples of 1<<32 microseconds.
+ *
+ * @return The current time in microseconds according to the FPGA (since FPGA
+ * reset) as a 64 bit number.
+ */
+uint64_t HAL_ExpandFPGATime(uint32_t unexpanded_lower, int32_t* status);
+
+/**
* Call this to start up HAL. This is required for robot programs.
*
* This must be called before any other HAL functions. Failure to do so will
@@ -135,34 +149,6 @@
*/
HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode);
-// ifdef's definition is to allow for default parameters in C++.
-#ifdef __cplusplus
-/**
- * Reports a hardware usage to the HAL.
- *
- * @param resource the used resource
- * @param instanceNumber the instance of the resource
- * @param context a user specified context index
- * @param feature a user specified feature string
- * @return the index of the added value in NetComm
- */
-int64_t HAL_Report(int32_t resource, int32_t instanceNumber,
- int32_t context = 0, const char* feature = nullptr);
-#else
-
-/**
- * Reports a hardware usage to the HAL.
- *
- * @param resource the used resource
- * @param instanceNumber the instance of the resource
- * @param context a user specified context index
- * @param feature a user specified feature string
- * @return the index of the added value in NetComm
- */
-int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context,
- const char* feature);
-#endif
-
#ifdef __cplusplus
} // extern "C"
#endif
diff --git a/hal/src/main/native/include/hal/Notifier.h b/hal/src/main/native/include/hal/Notifier.h
index 27f20e3..6b8e39f 100644
--- a/hal/src/main/native/include/hal/Notifier.h
+++ b/hal/src/main/native/include/hal/Notifier.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -32,6 +32,15 @@
HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status);
/**
+ * Sets the name of a notifier.
+ *
+ * @param notifierHandle the notifier handle
+ * @param name name
+ */
+void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
+ int32_t* status);
+
+/**
* Stops a notifier from running.
*
* This will cause any call into HAL_WaitForNotifierAlarm to return.
diff --git a/hal/src/main/native/include/hal/PDP.h b/hal/src/main/native/include/hal/PDP.h
index 50873f8..c80e8b6 100644
--- a/hal/src/main/native/include/hal/PDP.h
+++ b/hal/src/main/native/include/hal/PDP.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -80,6 +80,17 @@
int32_t* status);
/**
+ * Gets the current of all 16 channels on the PDP.
+ *
+ * The array must be large enough to hold all channels.
+ *
+ * @param handle the module handle
+ * @param current the currents (output)
+ */
+void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents,
+ int32_t* status);
+
+/**
* Gets the total current of the PDP.
*
* @param handle the module handle
diff --git a/hal/src/main/native/include/hal/Ports.h b/hal/src/main/native/include/hal/Ports.h
index 9b29817..584bc4f 100644
--- a/hal/src/main/native/include/hal/Ports.h
+++ b/hal/src/main/native/include/hal/Ports.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -144,6 +144,20 @@
* @return the number of PDP channels
*/
int32_t HAL_GetNumPDPChannels(void);
+
+/**
+ * Gets the number of duty cycle inputs in the current system.
+ *
+ * @return the number of Duty Cycle inputs
+ */
+int32_t HAL_GetNumDutyCycles(void);
+
+/**
+ * Gets the number of addressable LED generators in the current system.
+ *
+ * @return the number of Addressable LED generators
+ */
+int32_t HAL_GetNumAddressableLEDs(void);
#ifdef __cplusplus
} // extern "C"
#endif
diff --git a/hal/src/main/native/include/hal/Types.h b/hal/src/main/native/include/hal/Types.h
index 5ce6009..1ebbe2d 100644
--- a/hal/src/main/native/include/hal/Types.h
+++ b/hal/src/main/native/include/hal/Types.h
@@ -57,6 +57,12 @@
typedef HAL_Handle HAL_SimValueHandle;
+typedef HAL_Handle HAL_DMAHandle;
+
+typedef HAL_Handle HAL_DutyCycleHandle;
+
+typedef HAL_Handle HAL_AddressableLEDHandle;
+
typedef HAL_CANHandle HAL_PDPHandle;
typedef int32_t HAL_Bool;
diff --git a/hal/src/main/native/include/hal/handles/HandlesInternal.h b/hal/src/main/native/include/hal/handles/HandlesInternal.h
index 5340d82..511433e 100644
--- a/hal/src/main/native/include/hal/handles/HandlesInternal.h
+++ b/hal/src/main/native/include/hal/handles/HandlesInternal.h
@@ -66,6 +66,9 @@
SimulationJni = 18,
CAN = 19,
SerialPort = 20,
+ DutyCycle = 21,
+ DMA = 22,
+ AddressableLED = 23,
};
/**
diff --git a/hal/src/main/native/include/mockdata/AddressableLEDData.h b/hal/src/main/native/include/mockdata/AddressableLEDData.h
new file mode 100644
index 0000000..0d8f3f3
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/AddressableLEDData.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "NotifyListener.h"
+#include "hal/AddressableLEDTypes.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetAddressableLEDData(int32_t index);
+
+int32_t HALSIM_RegisterAddressableLEDInitializedCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAddressableLEDInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetAddressableLEDInitialized(int32_t index);
+void HALSIM_SetAddressableLEDInitialized(int32_t index, HAL_Bool initialized);
+
+int32_t HALSIM_RegisterAddressableLEDOutputPortCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAddressableLEDOutputPortCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetAddressableLEDOutputPort(int32_t index);
+void HALSIM_SetAddressableLEDOutputPort(int32_t index, int32_t outputPort);
+
+int32_t HALSIM_RegisterAddressableLEDLengthCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAddressableLEDLengthCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetAddressableLEDLength(int32_t index);
+void HALSIM_SetAddressableLEDLength(int32_t index, int32_t length);
+
+int32_t HALSIM_RegisterAddressableLEDRunningCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAddressableLEDRunningCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetAddressableLEDRunning(int32_t index);
+void HALSIM_SetAddressableLEDRunning(int32_t index, HAL_Bool running);
+
+int32_t HALSIM_RegisterAddressableLEDDataCallback(
+ int32_t index, HAL_ConstBufferCallback callback, void* param);
+void HALSIM_CancelAddressableLEDDataCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetAddressableLEDData(int32_t index,
+ struct HAL_AddressableLEDData* data);
+void HALSIM_SetAddressableLEDData(int32_t index,
+ const struct HAL_AddressableLEDData* data,
+ int32_t length);
+
+void HALSIM_RegisterAddressableLEDAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/AnalogTriggerData.h b/hal/src/main/native/include/mockdata/AnalogTriggerData.h
index 7f06c2e..566f2f5 100644
--- a/hal/src/main/native/include/mockdata/AnalogTriggerData.h
+++ b/hal/src/main/native/include/mockdata/AnalogTriggerData.h
@@ -13,6 +13,7 @@
enum HALSIM_AnalogTriggerMode : int32_t {
HALSIM_AnalogTriggerUnassigned,
HALSIM_AnalogTriggerFiltered,
+ HALSIM_AnalogTriggerDutyCycle,
HALSIM_AnalogTriggerAveraged
};
diff --git a/hal/src/main/native/include/mockdata/DutyCycleData.h b/hal/src/main/native/include/mockdata/DutyCycleData.h
new file mode 100644
index 0000000..b97b355
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/DutyCycleData.h
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "NotifyListener.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetDutyCycleData(int32_t index);
+int32_t HALSIM_GetDutyCycleDigitalChannel(int32_t index);
+
+int32_t HALSIM_RegisterDutyCycleInitializedCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelDutyCycleInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetDutyCycleInitialized(int32_t index);
+void HALSIM_SetDutyCycleInitialized(int32_t index, HAL_Bool initialized);
+
+HAL_SimDeviceHandle HALSIM_GetDutyCycleSimDevice(int32_t index);
+
+int32_t HALSIM_RegisterDutyCycleOutputCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelDutyCycleOutputCallback(int32_t index, int32_t uid);
+double HALSIM_GetDutyCycleOutput(int32_t index);
+void HALSIM_SetDutyCycleOutput(int32_t index, double output);
+
+int32_t HALSIM_RegisterDutyCycleFrequencyCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelDutyCycleFrequencyCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetDutyCycleFrequency(int32_t index);
+void HALSIM_SetDutyCycleFrequency(int32_t index, int32_t frequency);
+
+void HALSIM_RegisterDutyCycleAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/MockHooks.h b/hal/src/main/native/include/mockdata/MockHooks.h
index 02401b0..318ff21 100644
--- a/hal/src/main/native/include/mockdata/MockHooks.h
+++ b/hal/src/main/native/include/mockdata/MockHooks.h
@@ -14,6 +14,10 @@
void HALSIM_SetProgramStarted(void);
HAL_Bool HALSIM_GetProgramStarted(void);
void HALSIM_RestartTiming(void);
+void HALSIM_PauseTiming(void);
+void HALSIM_ResumeTiming(void);
+HAL_Bool HALSIM_IsTimingPaused(void);
+void HALSIM_StepTiming(uint64_t delta);
typedef int32_t (*HALSIM_SendErrorHandler)(
HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode, const char* details,
diff --git a/hal/src/main/native/include/mockdata/NotifierData.h b/hal/src/main/native/include/mockdata/NotifierData.h
new file mode 100644
index 0000000..b1ed50f
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/NotifierData.h
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct HALSIM_NotifierInfo {
+ HAL_NotifierHandle handle;
+ char name[64];
+ uint64_t timeout;
+ HAL_Bool running;
+};
+
+uint64_t HALSIM_GetNextNotifierTimeout(void);
+
+int32_t HALSIM_GetNumNotifiers(void);
+
+/**
+ * Gets detailed information about each notifier.
+ *
+ * @param arr array of information to be filled
+ * @param size size of arr
+ * @return Number of notifiers; note: may be larger than passed-in size
+ */
+int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/PCMData.h b/hal/src/main/native/include/mockdata/PCMData.h
index 74a591a..66b1ec2 100644
--- a/hal/src/main/native/include/mockdata/PCMData.h
+++ b/hal/src/main/native/include/mockdata/PCMData.h
@@ -74,6 +74,9 @@
double HALSIM_GetPCMCompressorCurrent(int32_t index);
void HALSIM_SetPCMCompressorCurrent(int32_t index, double compressorCurrent);
+void HALSIM_GetPCMAllSolenoids(int32_t index, uint8_t* values);
+void HALSIM_SetPCMAllSolenoids(int32_t index, uint8_t values);
+
void HALSIM_RegisterPCMAllNonSolenoidCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param,
diff --git a/hal/src/main/native/include/mockdata/PDPData.h b/hal/src/main/native/include/mockdata/PDPData.h
index a25b66d..8315e3c 100644
--- a/hal/src/main/native/include/mockdata/PDPData.h
+++ b/hal/src/main/native/include/mockdata/PDPData.h
@@ -46,6 +46,9 @@
double HALSIM_GetPDPCurrent(int32_t index, int32_t channel);
void HALSIM_SetPDPCurrent(int32_t index, int32_t channel, double current);
+void HALSIM_GetPDPAllCurrents(int32_t index, double* currents);
+void HALSIM_SetPDPAllCurrents(int32_t index, const double* currents);
+
void HALSIM_RegisterPDPAllNonCurrentCallbacks(int32_t index, int32_t channel,
HAL_NotifyCallback callback,
void* param,
diff --git a/hal/src/main/native/include/simulation/DutyCycleSim.h b/hal/src/main/native/include/simulation/DutyCycleSim.h
new file mode 100644
index 0000000..f55dfc9
--- /dev/null
+++ b/hal/src/main/native/include/simulation/DutyCycleSim.h
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/DutyCycleData.h"
+
+namespace frc {
+namespace sim {
+class DutyCycleSim {
+ public:
+ explicit DutyCycleSim(int index) { m_index = index; }
+
+ std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelDutyCycleInitializedCallback);
+ store->SetUid(HALSIM_RegisterDutyCycleInitializedCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetInitialized() const {
+ return HALSIM_GetDutyCycleInitialized(m_index);
+ }
+
+ void SetInitialized(bool initialized) {
+ HALSIM_SetDutyCycleInitialized(m_index, initialized);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterFrequencyCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelDutyCycleFrequencyCallback);
+ store->SetUid(HALSIM_RegisterDutyCycleFrequencyCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int GetFrequency() const { return HALSIM_GetDutyCycleFrequency(m_index); }
+
+ void SetFrequency(int count) { HALSIM_SetDutyCycleFrequency(m_index, count); }
+
+ std::unique_ptr<CallbackStore> RegisterOutputCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelDutyCycleOutputCallback);
+ store->SetUid(HALSIM_RegisterDutyCycleOutputCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetOutput() const { return HALSIM_GetDutyCycleOutput(m_index); }
+
+ void SetOutput(double period) { HALSIM_SetDutyCycleOutput(m_index, period); }
+
+ void ResetData() { HALSIM_ResetDutyCycleData(m_index); }
+
+ private:
+ int m_index;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/PCMSim.h b/hal/src/main/native/include/simulation/PCMSim.h
index 3f3ca9b..b6fcd5a 100644
--- a/hal/src/main/native/include/simulation/PCMSim.h
+++ b/hal/src/main/native/include/simulation/PCMSim.h
@@ -138,6 +138,16 @@
HALSIM_SetPCMCompressorCurrent(m_index, compressorCurrent);
}
+ uint8_t GetAllSolenoidOutputs() {
+ uint8_t ret = 0;
+ HALSIM_GetPCMAllSolenoids(m_index, &ret);
+ return ret;
+ }
+
+ void SetAllSolenoidOutputs(uint8_t outputs) {
+ HALSIM_SetPCMAllSolenoids(m_index, outputs);
+ }
+
void ResetData() { HALSIM_ResetPCMData(m_index); }
private:
diff --git a/hal/src/main/native/include/simulation/PDPSim.h b/hal/src/main/native/include/simulation/PDPSim.h
index e3ffd4b..72d8233 100644
--- a/hal/src/main/native/include/simulation/PDPSim.h
+++ b/hal/src/main/native/include/simulation/PDPSim.h
@@ -79,6 +79,14 @@
HALSIM_SetPDPCurrent(m_index, channel, current);
}
+ void GetAllCurrents(double* currents) {
+ HALSIM_GetPDPAllCurrents(m_index, currents);
+ }
+
+ void SetAllCurrents(const double* currents) {
+ HALSIM_SetPDPAllCurrents(m_index, currents);
+ }
+
void ResetData() { HALSIM_ResetPDPData(m_index); }
private:
diff --git a/hal/src/main/native/include/simulation/SimDeviceSim.h b/hal/src/main/native/include/simulation/SimDeviceSim.h
new file mode 100644
index 0000000..5705a08
--- /dev/null
+++ b/hal/src/main/native/include/simulation/SimDeviceSim.h
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "CallbackStore.h"
+#include "hal/SimDevice.h"
+#include "mockdata/SimDeviceData.h"
+
+namespace frc {
+namespace sim {
+class SimDeviceSim {
+ public:
+ explicit SimDeviceSim(const char* name)
+ : m_handle{HALSIM_GetSimDeviceHandle(name)} {}
+
+ hal::SimValue GetValue(const char* name) const {
+ return HALSIM_GetSimValueHandle(m_handle, name);
+ }
+
+ hal::SimDouble GetDouble(const char* name) const {
+ return HALSIM_GetSimValueHandle(m_handle, name);
+ }
+
+ hal::SimEnum GetEnum(const char* name) const {
+ return HALSIM_GetSimValueHandle(m_handle, name);
+ }
+
+ hal::SimBoolean GetBoolean(const char* name) const {
+ return HALSIM_GetSimValueHandle(m_handle, name);
+ }
+
+ static std::vector<std::string> GetEnumOptions(hal::SimEnum val) {
+ int32_t numOptions;
+ const char** options = HALSIM_GetSimValueEnumOptions(val, &numOptions);
+ std::vector<std::string> rv;
+ rv.reserve(numOptions);
+ for (int32_t i = 0; i < numOptions; ++i) rv.emplace_back(options[i]);
+ return rv;
+ }
+
+ template <typename F>
+ void EnumerateValues(F callback) const {
+ return HALSIM_EnumerateSimValues(
+ m_handle, &callback,
+ [](const char* name, void* param, HAL_SimValueHandle handle,
+ HAL_Bool readonly, const struct HAL_Value* value) {
+ std::invoke(*static_cast<F*>(param), name, handle, readonly, value);
+ });
+ }
+
+ operator HAL_SimDeviceHandle() const { return m_handle; }
+
+ template <typename F>
+ static void EnumerateDevices(const char* prefix, F callback) {
+ return HALSIM_EnumerateSimDevices(
+ prefix, &callback,
+ [](const char* name, void* param, HAL_SimDeviceHandle handle) {
+ std::invoke(*static_cast<F*>(param), name, handle);
+ });
+ }
+
+ static void ResetData() { HALSIM_ResetSimDeviceData(); }
+
+ private:
+ HAL_SimDeviceHandle m_handle;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/sim/AddressableLED.cpp b/hal/src/main/native/sim/AddressableLED.cpp
new file mode 100644
index 0000000..70d3f6f
--- /dev/null
+++ b/hal/src/main/native/sim/AddressableLED.cpp
@@ -0,0 +1,168 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/AddressableLED.h"
+
+#include "DigitalInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+#include "mockdata/AddressableLEDDataInternal.h"
+
+using namespace hal;
+
+namespace {
+struct AddressableLED {
+ uint8_t index;
+};
+} // namespace
+
+static LimitedHandleResource<HAL_AddressableLEDHandle, AddressableLED,
+ kNumAddressableLEDs,
+ HAL_HandleEnum::AddressableLED>* ledHandles;
+
+namespace hal {
+namespace init {
+void InitializeAddressableLED() {
+ static LimitedHandleResource<HAL_AddressableLEDHandle, AddressableLED,
+ kNumAddressableLEDs,
+ HAL_HandleEnum::AddressableLED>
+ dcH;
+ ledHandles = &dcH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+HAL_AddressableLEDHandle HAL_InitializeAddressableLED(
+ HAL_DigitalHandle outputPort, int32_t* status) {
+ hal::init::CheckInit();
+
+ auto digitalPort =
+ hal::digitalChannelHandles->Get(outputPort, hal::HAL_HandleEnum::PWM);
+
+ if (!digitalPort) {
+ // If DIO was passed, channel error, else generic error
+ if (getHandleType(outputPort) == hal::HAL_HandleEnum::DIO) {
+ *status = HAL_LED_CHANNEL_ERROR;
+ } else {
+ *status = HAL_HANDLE_ERROR;
+ }
+ return HAL_kInvalidHandle;
+ }
+
+ if (digitalPort->channel >= kNumPWMHeaders) {
+ *status = HAL_LED_CHANNEL_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ HAL_AddressableLEDHandle handle = ledHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+
+ auto led = ledHandles->Get(handle);
+ if (!led) { // would only occur on thread issue
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ int16_t index = getHandleIndex(handle);
+ SimAddressableLEDData[index].outputPort = digitalPort->channel;
+ SimAddressableLEDData[index].length = 1;
+ SimAddressableLEDData[index].running = false;
+ SimAddressableLEDData[index].initialized = true;
+ led->index = index;
+ return handle;
+}
+
+void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle) {
+ auto led = ledHandles->Get(handle);
+ ledHandles->Free(handle);
+ if (!led) return;
+ SimAddressableLEDData[led->index].running = false;
+ SimAddressableLEDData[led->index].initialized = false;
+}
+
+void HAL_SetAddressableLEDOutputPort(HAL_AddressableLEDHandle handle,
+ HAL_DigitalHandle outputPort,
+ int32_t* status) {
+ auto led = ledHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (auto port = digitalChannelHandles->Get(outputPort, HAL_HandleEnum::PWM)) {
+ SimAddressableLEDData[led->index].outputPort = port->channel;
+ } else {
+ SimAddressableLEDData[led->index].outputPort = -1;
+ }
+}
+
+void HAL_SetAddressableLEDLength(HAL_AddressableLEDHandle handle,
+ int32_t length, int32_t* status) {
+ auto led = ledHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (length > HAL_kAddressableLEDMaxLength) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+ SimAddressableLEDData[led->index].length = length;
+}
+
+void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle,
+ const struct HAL_AddressableLEDData* data,
+ int32_t length, int32_t* status) {
+ auto led = ledHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (length > SimAddressableLEDData[led->index].length) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+ SimAddressableLEDData[led->index].SetData(data, length);
+}
+
+void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle,
+ int32_t lowTime0NanoSeconds,
+ int32_t highTime0NanoSeconds,
+ int32_t lowTime1NanoSeconds,
+ int32_t highTime1NanoSeconds,
+ int32_t* status) {}
+
+void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle,
+ int32_t syncTimeMicroSeconds,
+ int32_t* status) {}
+
+void HAL_StartAddressableLEDOutput(HAL_AddressableLEDHandle handle,
+ int32_t* status) {
+ auto led = ledHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ SimAddressableLEDData[led->index].running = true;
+}
+
+void HAL_StopAddressableLEDOutput(HAL_AddressableLEDHandle handle,
+ int32_t* status) {
+ auto led = ledHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ SimAddressableLEDData[led->index].running = false;
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/AnalogInput.cpp b/hal/src/main/native/sim/AnalogInput.cpp
index dc56403..8ecde74 100644
--- a/hal/src/main/native/sim/AnalogInput.cpp
+++ b/hal/src/main/native/sim/AnalogInput.cpp
@@ -165,6 +165,15 @@
return SimAnalogInData[port->channel].voltage;
}
+
+double HAL_GetAnalogValueToVolts(HAL_AnalogInputHandle analogPortHandle,
+ int32_t rawValue, int32_t* status) {
+ int32_t LSBWeight = HAL_GetAnalogLSBWeight(analogPortHandle, status);
+ int32_t offset = HAL_GetAnalogOffset(analogPortHandle, status);
+ double voltage = LSBWeight * 1.0e-9 * rawValue - offset * 1.0e-9;
+ return voltage;
+}
+
double HAL_GetAnalogAverageVoltage(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
auto port = analogInputHandles->Get(analogPortHandle);
diff --git a/hal/src/main/native/sim/AnalogTrigger.cpp b/hal/src/main/native/sim/AnalogTrigger.cpp
index 53b165c..3ddacee 100644
--- a/hal/src/main/native/sim/AnalogTrigger.cpp
+++ b/hal/src/main/native/sim/AnalogTrigger.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -63,7 +63,7 @@
extern "C" {
HAL_AnalogTriggerHandle HAL_InitializeAnalogTrigger(
- HAL_AnalogInputHandle portHandle, int32_t* index, int32_t* status) {
+ HAL_AnalogInputHandle portHandle, int32_t* status) {
hal::init::CheckInit();
// ensure we are given a valid and active AnalogInput handle
auto analog_port = analogInputHandles->Get(portHandle);
@@ -83,7 +83,6 @@
}
trigger->analogHandle = portHandle;
trigger->index = static_cast<uint8_t>(getHandleIndex(handle));
- *index = trigger->index;
SimAnalogTriggerData[trigger->index].initialized = true;
@@ -92,6 +91,12 @@
return handle;
}
+HAL_AnalogTriggerHandle HAL_InitializeAnalogTriggerDutyCycle(
+ HAL_DutyCycleHandle dutyCycleHandle, int32_t* status) {
+ *status = HAL_SIM_NOT_SUPPORTED;
+ return HAL_kInvalidHandle;
+}
+
void HAL_CleanAnalogTrigger(HAL_AnalogTriggerHandle analogTriggerHandle,
int32_t* status) {
auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
@@ -133,6 +138,13 @@
SimAnalogTriggerData[trigger->index].triggerUpperBound = trigUpper;
SimAnalogTriggerData[trigger->index].triggerLowerBound = trigLower;
}
+
+void HAL_SetAnalogTriggerLimitsDutyCycle(
+ HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
+ int32_t* status) {
+ *status = HAL_SIM_NOT_SUPPORTED;
+}
+
void HAL_SetAnalogTriggerLimitsVoltage(
HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
int32_t* status) {
@@ -158,7 +170,7 @@
AnalogTriggerData* triggerData = &SimAnalogTriggerData[trigger->index];
- if (triggerData->triggerMode.Get() == HALSIM_AnalogTriggerFiltered) {
+ if (triggerData->triggerMode.Get() != HALSIM_AnalogTriggerUnassigned) {
*status = INCOMPATIBLE_STATE;
return;
}
@@ -167,6 +179,7 @@
: HALSIM_AnalogTriggerUnassigned;
triggerData->triggerMode = setVal;
}
+
void HAL_SetAnalogTriggerFiltered(HAL_AnalogTriggerHandle analogTriggerHandle,
HAL_Bool useFilteredValue, int32_t* status) {
auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
@@ -177,12 +190,12 @@
AnalogTriggerData* triggerData = &SimAnalogTriggerData[trigger->index];
- if (triggerData->triggerMode.Get() == HALSIM_AnalogTriggerAveraged) {
+ if (triggerData->triggerMode.Get() != HALSIM_AnalogTriggerUnassigned) {
*status = INCOMPATIBLE_STATE;
return;
}
- auto setVal = useFilteredValue ? HALSIM_AnalogTriggerAveraged
+ auto setVal = useFilteredValue ? HALSIM_AnalogTriggerFiltered
: HALSIM_AnalogTriggerUnassigned;
triggerData->triggerMode = setVal;
}
@@ -258,4 +271,15 @@
return false;
}
}
+
+int32_t HAL_GetAnalogTriggerFPGAIndex(
+ HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+ return trigger->index;
+}
+
} // extern "C"
diff --git a/hal/src/main/native/sim/CAN.cpp b/hal/src/main/native/sim/CAN.cpp
index d55eafd..1e7af73 100644
--- a/hal/src/main/native/sim/CAN.cpp
+++ b/hal/src/main/native/sim/CAN.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -26,8 +26,16 @@
void HAL_CAN_ReceiveMessage(uint32_t* messageID, uint32_t messageIDMask,
uint8_t* data, uint8_t* dataSize,
uint32_t* timeStamp, int32_t* status) {
+ // Use a data size of 42 as call check. Difficult to add check to invoke
+ // handler
+ *dataSize = 42;
+ auto tmpStatus = *status;
SimCanData->receiveMessage(messageID, messageIDMask, data, dataSize,
timeStamp, status);
+ // If no handler invoked, return message not found
+ if (*dataSize == 42 && *status == tmpStatus) {
+ *status = HAL_ERR_CANSessionMux_MessageNotFound;
+ }
}
void HAL_CAN_OpenStreamSession(uint32_t* sessionHandle, uint32_t messageID,
uint32_t messageIDMask, uint32_t maxMessages,
diff --git a/hal/src/main/native/sim/DMA.cpp b/hal/src/main/native/sim/DMA.cpp
new file mode 100644
index 0000000..cea5a29
--- /dev/null
+++ b/hal/src/main/native/sim/DMA.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/DMA.h"
+
+extern "C" {
+HAL_DMAHandle HAL_InitializeDMA(int32_t* status) { return HAL_kInvalidHandle; }
+void HAL_FreeDMA(HAL_DMAHandle handle) {}
+
+void HAL_SetDMAPause(HAL_DMAHandle handle, HAL_Bool pause, int32_t* status) {}
+void HAL_SetDMARate(HAL_DMAHandle handle, int32_t cycles, int32_t* status) {}
+
+void HAL_AddDMAEncoder(HAL_DMAHandle handle, HAL_EncoderHandle encoderHandle,
+ int32_t* status) {}
+void HAL_AddDMAEncoderPeriod(HAL_DMAHandle handle,
+ HAL_EncoderHandle encoderHandle, int32_t* status) {
+}
+void HAL_AddDMACounter(HAL_DMAHandle handle, HAL_CounterHandle counterHandle,
+ int32_t* status) {}
+void HAL_AddDMACounterPeriod(HAL_DMAHandle handle,
+ HAL_CounterHandle counterHandle, int32_t* status) {
+}
+void HAL_AddDMADigitalSource(HAL_DMAHandle handle,
+ HAL_Handle digitalSourceHandle, int32_t* status) {}
+void HAL_AddDMAAnalogInput(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle, int32_t* status) {}
+
+void HAL_AddDMAAveragedAnalogInput(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status) {}
+
+void HAL_AddDMAAnalogAccumulator(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status) {}
+
+void HAL_AddDMADutyCycle(HAL_DMAHandle handle,
+ HAL_DutyCycleHandle dutyCycleHandle, int32_t* status) {
+}
+
+void HAL_SetDMAExternalTrigger(HAL_DMAHandle handle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_Bool rising, HAL_Bool falling,
+ int32_t* status) {}
+
+void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status) {}
+void HAL_StopDMA(HAL_DMAHandle handle, int32_t* status) {}
+
+void* HAL_GetDMADirectPointer(HAL_DMAHandle handle) { return nullptr; }
+
+enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer,
+ HAL_DMASample* dmaSample,
+ int32_t timeoutMs,
+ int32_t* remainingOut,
+ int32_t* status) {
+ return HAL_DMA_ERROR;
+}
+
+enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle,
+ HAL_DMASample* dmaSample, int32_t timeoutMs,
+ int32_t* remainingOut, int32_t* status) {
+ return HAL_DMA_ERROR;
+}
+
+// Sampling Code
+uint64_t HAL_GetDMASampleTime(const HAL_DMASample* dmaSample, int32_t* status) {
+ return 0;
+}
+
+int32_t HAL_GetDMASampleEncoderRaw(const HAL_DMASample* dmaSample,
+ HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ return 0;
+}
+
+int32_t HAL_GetDMASampleCounter(const HAL_DMASample* dmaSample,
+ HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ return 0;
+}
+
+int32_t HAL_GetDMASampleEncoderPeriodRaw(const HAL_DMASample* dmaSample,
+ HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ return 0;
+}
+
+int32_t HAL_GetDMASampleCounterPeriod(const HAL_DMASample* dmaSample,
+ HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ return 0;
+}
+HAL_Bool HAL_GetDMASampleDigitalSource(const HAL_DMASample* dmaSample,
+ HAL_Handle dSourceHandle,
+ int32_t* status) {
+ return 0;
+}
+int32_t HAL_GetDMASampleAnalogInputRaw(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status) {
+ return 0;
+}
+
+int32_t HAL_GetDMASampleAveragedAnalogInputRaw(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status) {
+ return 0;
+}
+
+void HAL_GetDMASampleAnalogAccumulator(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int64_t* count, int64_t* value,
+ int32_t* status) {}
+
+int32_t HAL_GetDMASampleDutyCycleOutputRaw(const HAL_DMASample* dmaSample,
+ HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ return 0;
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp
index a17994e..e8404cf 100644
--- a/hal/src/main/native/sim/DriverStation.cpp
+++ b/hal/src/main/native/sim/DriverStation.cpp
@@ -210,7 +210,11 @@
}
#endif
-HAL_Bool HAL_IsNewControlData(void) {
+static int& GetThreadLocalLastCount() {
+ // There is a rollover error condition here. At Packet# = n * (uintmax), this
+ // will return false when instead it should return true. However, this at a
+ // 20ms rate occurs once every 2.7 years of DS connected runtime, so not
+ // worth the cycles to check.
#ifdef __APPLE__
pthread_once(&lastCountKeyOnce, InitLastCountKey);
int* lastCountPtr = static_cast<int*>(pthread_getspecific(lastCountKey));
@@ -223,13 +227,43 @@
#else
thread_local int lastCount{-1};
#endif
- // There is a rollover error condition here. At Packet# = n * (uintmax), this
- // will return false when instead it should return true. However, this at a
- // 20ms rate occurs once every 2.7 years of DS connected runtime, so not
- // worth the cycles to check.
+ return lastCount;
+}
+
+HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout) {
+ int& lastCount = GetThreadLocalLastCount();
+ std::unique_lock lock(newDSDataAvailableMutex);
+ int currentCount = newDSDataAvailableCounter;
+ if (lastCount != currentCount) {
+ lastCount = currentCount;
+ return true;
+ }
+
+ if (isFinalized.load()) {
+ return false;
+ }
+
+ auto timeoutTime =
+ std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+
+ while (newDSDataAvailableCounter == currentCount) {
+ if (timeout > 0) {
+ auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime);
+ if (timedOut == std::cv_status::timeout) {
+ return false;
+ }
+ } else {
+ newDSDataAvailableCond->wait(lock);
+ }
+ }
+ return true;
+}
+
+HAL_Bool HAL_IsNewControlData(void) {
+ int& lastCount = GetThreadLocalLastCount();
int currentCount = 0;
{
- std::unique_lock lock(newDSDataAvailableMutex);
+ std::scoped_lock lock(newDSDataAvailableMutex);
currentCount = newDSDataAvailableCounter;
}
if (lastCount == currentCount) return false;
diff --git a/hal/src/main/native/sim/DutyCycle.cpp b/hal/src/main/native/sim/DutyCycle.cpp
new file mode 100644
index 0000000..0e14eeb
--- /dev/null
+++ b/hal/src/main/native/sim/DutyCycle.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/DutyCycle.h"
+
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+#include "mockdata/DutyCycleDataInternal.h"
+
+using namespace hal;
+
+namespace {
+struct DutyCycle {
+ uint8_t index;
+};
+struct Empty {};
+} // namespace
+
+static LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
+ HAL_HandleEnum::DutyCycle>* dutyCycleHandles;
+
+namespace hal {
+namespace init {
+void InitializeDutyCycle() {
+ static LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
+ HAL_HandleEnum::DutyCycle>
+ dcH;
+ dutyCycleHandles = &dcH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType triggerType,
+ int32_t* status) {
+ hal::init::CheckInit();
+
+ HAL_DutyCycleHandle handle = dutyCycleHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+
+ auto dutyCycle = dutyCycleHandles->Get(handle);
+ if (dutyCycle == nullptr) { // would only occur on thread issue
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ int16_t index = getHandleIndex(handle);
+ SimDutyCycleData[index].digitalChannel = getHandleIndex(digitalSourceHandle);
+ SimDutyCycleData[index].initialized = true;
+ SimDutyCycleData[index].simDevice = 0;
+ dutyCycle->index = index;
+ return handle;
+}
+void HAL_FreeDutyCycle(HAL_DutyCycleHandle dutyCycleHandle) {
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ dutyCycleHandles->Free(dutyCycleHandle);
+ if (dutyCycle == nullptr) return;
+ SimDutyCycleData[dutyCycle->index].initialized = false;
+}
+
+void HAL_SetDutyCycleSimDevice(HAL_EncoderHandle handle,
+ HAL_SimDeviceHandle device) {
+ auto dutyCycle = dutyCycleHandles->Get(handle);
+ if (dutyCycle == nullptr) return;
+ SimDutyCycleData[dutyCycle->index].simDevice = device;
+}
+
+int32_t HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ if (dutyCycle == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return SimDutyCycleData[dutyCycle->index].frequency;
+}
+double HAL_GetDutyCycleOutput(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ if (dutyCycle == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return SimDutyCycleData[dutyCycle->index].output;
+}
+int32_t HAL_GetDutyCycleOutputRaw(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ if (dutyCycle == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return SimDutyCycleData[dutyCycle->index].output *
+ HAL_GetDutyCycleOutputScaleFactor(dutyCycleHandle, status);
+}
+int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ return 4e7 - 1;
+}
+int32_t HAL_GetDutyCycleFPGAIndex(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ if (dutyCycle == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+ return dutyCycle->index;
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/Extensions.cpp b/hal/src/main/native/sim/Extensions.cpp
index e84c354..951ad10 100644
--- a/hal/src/main/native/sim/Extensions.cpp
+++ b/hal/src/main/native/sim/Extensions.cpp
@@ -41,6 +41,11 @@
} // namespace init
} // namespace hal
+static bool& GetShowNotFoundMessage() {
+ static bool showMsg = true;
+ return showMsg;
+}
+
extern "C" {
int HAL_LoadOneExtension(const char* library) {
@@ -91,8 +96,10 @@
wpi::SmallVector<wpi::StringRef, 2> libraries;
const char* e = std::getenv("HALSIM_EXTENSIONS");
if (!e) {
- wpi::outs() << "HAL Extensions: No extensions found\n";
- wpi::outs().flush();
+ if (GetShowNotFoundMessage()) {
+ wpi::outs() << "HAL Extensions: No extensions found\n";
+ wpi::outs().flush();
+ }
return rc;
}
wpi::StringRef env{e};
@@ -105,4 +112,8 @@
return rc;
}
+void HAL_SetShowExtensionsNotFoundMessages(HAL_Bool showMessage) {
+ GetShowNotFoundMessage() = showMessage;
+}
+
} // extern "C"
diff --git a/hal/src/main/native/sim/HAL.cpp b/hal/src/main/native/sim/HAL.cpp
index cf019cf..0dda617 100644
--- a/hal/src/main/native/sim/HAL.cpp
+++ b/hal/src/main/native/sim/HAL.cpp
@@ -25,6 +25,7 @@
namespace init {
void InitializeHAL() {
InitializeAccelerometerData();
+ InitializeAddressableLEDData();
InitializeAnalogGyroData();
InitializeAnalogInData();
InitializeAnalogOutData();
@@ -32,6 +33,7 @@
InitializeCanData();
InitializeCANAPI();
InitializeDigitalPWMData();
+ InitializeDutyCycleData();
InitializeDIOData();
InitializeDriverStationData();
InitializeEncoderData();
@@ -45,6 +47,7 @@
InitializeSPIAccelerometerData();
InitializeSPIData();
InitializeAccelerometer();
+ InitializeAddressableLED();
InitializeAnalogAccumulator();
InitializeAnalogGyro();
InitializeAnalogInput();
@@ -56,6 +59,7 @@
InitializeCounter();
InitializeDigitalInternal();
InitializeDIO();
+ InitializeDutyCycle();
InitializeDriverStation();
InitializeEncoder();
InitializeExtensions();
@@ -199,6 +203,12 @@
return HAL_PWM_SCALE_ERROR_MESSAGE;
case HAL_CAN_TIMEOUT:
return HAL_CAN_TIMEOUT_MESSAGE;
+ case HAL_SIM_NOT_SUPPORTED:
+ return HAL_SIM_NOT_SUPPORTED_MESSAGE;
+ case HAL_CAN_BUFFER_OVERRUN:
+ return HAL_CAN_BUFFER_OVERRUN_MESSAGE;
+ case HAL_LED_CHANNEL_ERROR:
+ return HAL_LED_CHANNEL_ERROR_MESSAGE;
default:
return "Unknown error status";
}
@@ -216,6 +226,28 @@
uint64_t HAL_GetFPGATime(int32_t* status) { return hal::GetFPGATime(); }
+uint64_t HAL_ExpandFPGATime(uint32_t unexpanded_lower, int32_t* status) {
+ // Capture the current FPGA time. This will give us the upper half of the
+ // clock.
+ uint64_t fpga_time = HAL_GetFPGATime(status);
+ if (*status != 0) return 0;
+
+ // Now, we need to detect the case where the lower bits rolled over after we
+ // sampled. In that case, the upper bits will be 1 bigger than they should
+ // be.
+
+ // Break it into lower and upper portions.
+ uint32_t lower = fpga_time & 0xffffffffull;
+ uint64_t upper = (fpga_time >> 32) & 0xffffffff;
+
+ // The time was sampled *before* the current time, so roll it back.
+ if (lower < unexpanded_lower) {
+ --upper;
+ }
+
+ return (upper << 32) + static_cast<uint64_t>(unexpanded_lower);
+}
+
HAL_Bool HAL_GetFPGAButton(int32_t* status) {
return SimRoboRioData[0].fpgaButton;
}
diff --git a/hal/src/main/native/sim/HALInitializer.h b/hal/src/main/native/sim/HALInitializer.h
index 4c91b40..c08df73 100644
--- a/hal/src/main/native/sim/HALInitializer.h
+++ b/hal/src/main/native/sim/HALInitializer.h
@@ -19,6 +19,7 @@
}
extern void InitializeAccelerometerData();
+extern void InitializeAddressableLEDData();
extern void InitializeAnalogGyroData();
extern void InitializeAnalogInData();
extern void InitializeAnalogOutData();
@@ -26,7 +27,9 @@
extern void InitializeCanData();
extern void InitializeCANAPI();
extern void InitializeDigitalPWMData();
+extern void InitializeDutyCycleData();
extern void InitializeDIOData();
+extern void InitializeDutyCycle();
extern void InitializeDriverStationData();
extern void InitializeEncoderData();
extern void InitializeI2CData();
@@ -39,6 +42,7 @@
extern void InitializeSPIAccelerometerData();
extern void InitializeSPIData();
extern void InitializeAccelerometer();
+extern void InitializeAddressableLED();
extern void InitializeAnalogAccumulator();
extern void InitializeAnalogGyro();
extern void InitializeAnalogInput();
diff --git a/hal/src/main/native/sim/MockHooks.cpp b/hal/src/main/native/sim/MockHooks.cpp
index 25c1d9f..59086ae 100644
--- a/hal/src/main/native/sim/MockHooks.cpp
+++ b/hal/src/main/native/sim/MockHooks.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -13,10 +13,12 @@
#include <wpi/timestamp.h>
#include "MockHooksInternal.h"
+#include "NotifierInternal.h"
static std::atomic<bool> programStarted{false};
static std::atomic<uint64_t> programStartTime{0};
+static std::atomic<uint64_t> programPauseTime{0};
namespace hal {
namespace init {
@@ -25,12 +27,32 @@
} // namespace hal
namespace hal {
-void RestartTiming() { programStartTime = wpi::Now(); }
+void RestartTiming() {
+ programStartTime = wpi::Now();
+ if (programPauseTime != 0) programPauseTime = programStartTime.load();
+}
+
+void PauseTiming() {
+ if (programPauseTime == 0) programPauseTime = wpi::Now();
+}
+
+void ResumeTiming() {
+ if (programPauseTime != 0) {
+ programStartTime += wpi::Now() - programPauseTime;
+ programPauseTime = 0;
+ }
+}
+
+bool IsTimingPaused() { return programPauseTime != 0; }
+
+void StepTiming(uint64_t delta) {
+ if (programPauseTime != 0) programPauseTime += delta;
+}
int64_t GetFPGATime() {
- auto now = wpi::Now();
- auto currentTime = now - programStartTime;
- return currentTime;
+ uint64_t curTime = programPauseTime;
+ if (curTime == 0) curTime = wpi::Now();
+ return curTime - programStartTime;
}
double GetFPGATimestamp() { return GetFPGATime() * 1.0e-6; }
@@ -56,4 +78,21 @@
HAL_Bool HALSIM_GetProgramStarted(void) { return GetProgramStarted(); }
void HALSIM_RestartTiming(void) { RestartTiming(); }
+
+void HALSIM_PauseTiming(void) {
+ PauseTiming();
+ PauseNotifiers();
+}
+
+void HALSIM_ResumeTiming(void) {
+ ResumeTiming();
+ ResumeNotifiers();
+}
+
+HAL_Bool HALSIM_IsTimingPaused(void) { return IsTimingPaused(); }
+
+void HALSIM_StepTiming(uint64_t delta) {
+ StepTiming(delta);
+ WakeupNotifiers();
+}
} // extern "C"
diff --git a/hal/src/main/native/sim/MockHooksInternal.h b/hal/src/main/native/sim/MockHooksInternal.h
index e8c09a9..a69e9bf 100644
--- a/hal/src/main/native/sim/MockHooksInternal.h
+++ b/hal/src/main/native/sim/MockHooksInternal.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -14,6 +14,14 @@
namespace hal {
void RestartTiming();
+void PauseTiming();
+
+void ResumeTiming();
+
+bool IsTimingPaused();
+
+void StepTiming(uint64_t delta);
+
int64_t GetFPGATime();
double GetFPGATimestamp();
diff --git a/hal/src/main/native/sim/Notifier.cpp b/hal/src/main/native/sim/Notifier.cpp
index 579540c..211f7b2 100644
--- a/hal/src/main/native/sim/Notifier.cpp
+++ b/hal/src/main/native/sim/Notifier.cpp
@@ -7,21 +7,27 @@
#include "hal/Notifier.h"
+#include <atomic>
#include <chrono>
+#include <cstdio>
+#include <cstring>
+#include <string>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
#include <wpi/timestamp.h>
#include "HALInitializer.h"
+#include "NotifierInternal.h"
#include "hal/HAL.h"
#include "hal/cpp/fpga_clock.h"
#include "hal/handles/UnlimitedHandleResource.h"
+#include "mockdata/NotifierData.h"
namespace {
struct Notifier {
+ std::string name;
uint64_t waitTime;
- bool updatedAlarm = false;
bool active = true;
bool running = false;
wpi::mutex mutex;
@@ -48,6 +54,7 @@
};
static NotifierHandleContainer* notifierHandles;
+static std::atomic<bool> notifiersPaused{false};
namespace hal {
namespace init {
@@ -56,6 +63,19 @@
notifierHandles = &nH;
}
} // namespace init
+
+void PauseNotifiers() { notifiersPaused = true; }
+
+void ResumeNotifiers() {
+ notifiersPaused = false;
+ WakeupNotifiers();
+}
+
+void WakeupNotifiers() {
+ notifierHandles->ForEach([](HAL_NotifierHandle handle, Notifier* notifier) {
+ notifier->cond.notify_all();
+ });
+}
} // namespace hal
extern "C" {
@@ -71,6 +91,14 @@
return handle;
}
+void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
+ int32_t* status) {
+ auto notifier = notifierHandles->Get(notifierHandle);
+ if (!notifier) return;
+ std::scoped_lock lock(notifier->mutex);
+ notifier->name = name;
+}
+
void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
if (!notifier) return;
@@ -105,7 +133,6 @@
std::scoped_lock lock(notifier->mutex);
notifier->waitTime = triggerTime;
notifier->running = true;
- notifier->updatedAlarm = true;
}
// We wake up any waiters to change how long they're sleeping for
@@ -131,29 +158,66 @@
std::unique_lock lock(notifier->mutex);
while (notifier->active) {
double waitTime;
- if (!notifier->running) {
+ if (!notifier->running || notifiersPaused) {
waitTime = (HAL_GetFPGATime(status) * 1e-6) + 1000.0;
// If not running, wait 1000 seconds
} else {
waitTime = notifier->waitTime * 1e-6;
}
- // Don't wait twice
- notifier->updatedAlarm = false;
-
auto timeoutTime =
hal::fpga_clock::epoch() + std::chrono::duration<double>(waitTime);
notifier->cond.wait_until(lock, timeoutTime);
- if (notifier->updatedAlarm) {
- notifier->updatedAlarm = false;
- continue;
- }
if (!notifier->running) continue;
if (!notifier->active) break;
+ uint64_t curTime = HAL_GetFPGATime(status);
+ if (curTime < notifier->waitTime) continue;
notifier->running = false;
- return HAL_GetFPGATime(status);
+ return curTime;
}
return 0;
}
+uint64_t HALSIM_GetNextNotifierTimeout(void) {
+ uint64_t timeout = UINT64_MAX;
+ notifierHandles->ForEach([&](HAL_NotifierHandle, Notifier* notifier) {
+ std::scoped_lock lock(notifier->mutex);
+ if (notifier->active && notifier->running && timeout > notifier->waitTime)
+ timeout = notifier->waitTime;
+ });
+ return timeout;
+}
+
+int32_t HALSIM_GetNumNotifiers(void) {
+ int32_t count = 0;
+ notifierHandles->ForEach([&](HAL_NotifierHandle, Notifier* notifier) {
+ std::scoped_lock lock(notifier->mutex);
+ if (notifier->active) ++count;
+ });
+ return count;
+}
+
+int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size) {
+ int32_t num = 0;
+ notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
+ std::scoped_lock lock(notifier->mutex);
+ if (!notifier->active) return;
+ if (num < size) {
+ arr[num].handle = handle;
+ if (notifier->name.empty()) {
+ std::snprintf(arr[num].name, sizeof(arr[num].name), "Notifier%d",
+ static_cast<int>(getHandleIndex(handle)));
+ } else {
+ std::strncpy(arr[num].name, notifier->name.c_str(),
+ sizeof(arr[num].name));
+ arr[num].name[sizeof(arr[num].name) - 1] = '\0';
+ }
+ arr[num].timeout = notifier->waitTime;
+ arr[num].running = notifier->running;
+ }
+ ++num;
+ });
+ return num;
+}
+
} // extern "C"
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp b/hal/src/main/native/sim/NotifierInternal.h
similarity index 64%
copy from wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
copy to hal/src/main/native/sim/NotifierInternal.h
index 8bd62ea..84232d2 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
+++ b/hal/src/main/native/sim/NotifierInternal.h
@@ -5,12 +5,10 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include "frc2/command/PrintCommand.h"
+#pragma once
-using namespace frc2;
-
-PrintCommand::PrintCommand(const wpi::Twine& message)
- : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
-}
-
-bool PrintCommand::RunsWhenDisabled() const { return true; }
+namespace hal {
+void PauseNotifiers();
+void ResumeNotifiers();
+void WakeupNotifiers();
+} // namespace hal
diff --git a/hal/src/main/native/sim/PDP.cpp b/hal/src/main/native/sim/PDP.cpp
index 07f4dcd..dbe09d4 100644
--- a/hal/src/main/native/sim/PDP.cpp
+++ b/hal/src/main/native/sim/PDP.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -78,6 +78,18 @@
}
return SimPDPData[module].current[channel];
}
+void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents,
+ int32_t* status) {
+ auto module = hal::can::GetCANModuleFromHandle(handle, status);
+ if (*status != 0) {
+ return;
+ }
+
+ auto& data = SimPDPData[module];
+ for (int i = 0; i < kNumPDPChannels; i++) {
+ currents[i] = data.current[i];
+ }
+}
double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) {
return 0.0;
}
diff --git a/hal/src/main/native/sim/Ports.cpp b/hal/src/main/native/sim/Ports.cpp
index f50304f..2f670b3 100644
--- a/hal/src/main/native/sim/Ports.cpp
+++ b/hal/src/main/native/sim/Ports.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -36,5 +36,6 @@
int32_t HAL_GetNumSolenoidChannels(void) { return kNumSolenoidChannels; }
int32_t HAL_GetNumPDPModules(void) { return kNumPDPModules; }
int32_t HAL_GetNumPDPChannels(void) { return kNumPDPChannels; }
-int32_t HAL_GetNumCanTalons(void) { return kNumCanTalons; }
+int32_t HAL_GetNumDutyCycles(void) { return kNumDutyCycles; }
+int32_t HAL_GetNumAddressableLEDs(void) { return kNumAddressableLEDs; }
} // extern "C"
diff --git a/hal/src/main/native/sim/PortsInternal.h b/hal/src/main/native/sim/PortsInternal.h
index 0b899f7..cfbf1e7 100644
--- a/hal/src/main/native/sim/PortsInternal.h
+++ b/hal/src/main/native/sim/PortsInternal.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -28,5 +28,6 @@
constexpr int32_t kNumSolenoidChannels = 8;
constexpr int32_t kNumPDPModules = 63;
constexpr int32_t kNumPDPChannels = 16;
-constexpr int32_t kNumCanTalons = 63;
+constexpr int32_t kNumDutyCycles = 8;
+constexpr int32_t kNumAddressableLEDs = 1;
} // namespace hal
diff --git a/hal/src/main/native/sim/Solenoid.cpp b/hal/src/main/native/sim/Solenoid.cpp
index d3f0c5d..46bd285 100644
--- a/hal/src/main/native/sim/Solenoid.cpp
+++ b/hal/src/main/native/sim/Solenoid.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -120,6 +120,15 @@
HALSIM_SetPCMSolenoidOutput(port->module, port->channel, value);
}
+
+void HAL_SetAllSolenoids(int32_t module, int32_t state, int32_t* status) {
+ for (int i = 0; i < kNumSolenoidChannels; i++) {
+ int set = state & 1;
+ HALSIM_SetPCMSolenoidOutput(module, i, set);
+ state >>= 1;
+ }
+}
+
int32_t HAL_GetPCMSolenoidBlackList(int32_t module, int32_t* status) {
return 0;
}
diff --git a/hal/src/main/native/sim/jni/AddressableLEDDataJNI.cpp b/hal/src/main/native/sim/jni/AddressableLEDDataJNI.cpp
new file mode 100644
index 0000000..530eae2
--- /dev/null
+++ b/hal/src/main/native/sim/jni/AddressableLEDDataJNI.cpp
@@ -0,0 +1,293 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "ConstBufferCallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI.h"
+#include "mockdata/AddressableLEDData.h"
+
+static_assert(sizeof(jbyte) * 4 == sizeof(HAL_AddressableLEDData));
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(
+ env, index, callback, initialNotify,
+ &HALSIM_RegisterAddressableLEDInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAddressableLEDInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAddressableLEDInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetAddressableLEDInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: registerOutputPortCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerOutputPortCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(
+ env, index, callback, initialNotify,
+ &HALSIM_RegisterAddressableLEDOutputPortCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: cancelOutputPortCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelOutputPortCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAddressableLEDOutputPortCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: getOutputPort
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getOutputPort
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAddressableLEDOutputPort(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: setOutputPort
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setOutputPort
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetAddressableLEDOutputPort(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: registerLengthCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerLengthCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAddressableLEDLengthCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: cancelLengthCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelLengthCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAddressableLEDLengthCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: getLength
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getLength
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAddressableLEDLength(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: setLength
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setLength
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetAddressableLEDLength(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: registerRunningCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerRunningCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAddressableLEDRunningCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: cancelRunningCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelRunningCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAddressableLEDRunningCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: getRunning
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getRunning
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAddressableLEDRunning(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: setRunning
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setRunning
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetAddressableLEDRunning(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: registerDataCallback
+ * Signature: (ILjava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerDataCallback
+ (JNIEnv* env, jclass, jint index, jobject callback)
+{
+ return sim::AllocateConstBufferCallback(
+ env, index, callback, &HALSIM_RegisterAddressableLEDDataCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: cancelDataCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelDataCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ sim::FreeConstBufferCallback(env, handle, index,
+ &HALSIM_CancelAddressableLEDDataCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: getData
+ * Signature: (I)[B
+ */
+JNIEXPORT jbyteArray JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getData
+ (JNIEnv* env, jclass, jint index)
+{
+ auto data =
+ std::make_unique<HAL_AddressableLEDData[]>(HAL_kAddressableLEDMaxLength);
+ int32_t length = HALSIM_GetAddressableLEDData(index, data.get());
+ return MakeJByteArray(
+ env, wpi::ArrayRef(reinterpret_cast<jbyte*>(data.get()), length * 4));
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: setData
+ * Signature: (I[B)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setData
+ (JNIEnv* env, jclass, jint index, jbyteArray arr)
+{
+ JByteArrayRef jArrRef{env, arr};
+ auto arrRef = jArrRef.array();
+ HALSIM_SetAddressableLEDData(
+ index, reinterpret_cast<const HAL_AddressableLEDData*>(arrRef.data()),
+ arrRef.size() / 4);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetAddressableLEDData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/DutyCycleDataJNI.cpp b/hal/src/main/native/sim/jni/DutyCycleDataJNI.cpp
new file mode 100644
index 0000000..d746ce1
--- /dev/null
+++ b/hal/src/main/native/sim/jni/DutyCycleDataJNI.cpp
@@ -0,0 +1,178 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI.h"
+#include "mockdata/DutyCycleData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_registerInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterDutyCycleInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_cancelInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelDutyCycleInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_getInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetDutyCycleInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_setInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetDutyCycleInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: registerFrequencyCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_registerFrequencyCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterDutyCycleFrequencyCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: cancelFrequencyCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_cancelFrequencyCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelDutyCycleFrequencyCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: getFrequency
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_getFrequency
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetDutyCycleFrequency(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: setFrequency
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_setFrequency
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetDutyCycleFrequency(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: registerOutputCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_registerOutputCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterDutyCycleOutputCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: cancelOutputCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_cancelOutputCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelDutyCycleOutputCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: getOutput
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_getOutput
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetDutyCycleOutput(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: setOutput
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_setOutput
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetDutyCycleOutput(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetDutyCycleData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/NotifierDataJNI.cpp b/hal/src/main/native/sim/jni/NotifierDataJNI.cpp
new file mode 100644
index 0000000..b59b6e0
--- /dev/null
+++ b/hal/src/main/native/sim/jni/NotifierDataJNI.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "edu_wpi_first_hal_sim_mockdata_NotifierDataJNI.h"
+#include "mockdata/NotifierData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_NotifierDataJNI
+ * Method: getNextTimeout
+ * Signature: ()J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_NotifierDataJNI_getNextTimeout
+ (JNIEnv*, jclass)
+{
+ return HALSIM_GetNextNotifierTimeout();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_NotifierDataJNI
+ * Method: getNumNotifiers
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_NotifierDataJNI_getNumNotifiers
+ (JNIEnv*, jclass)
+{
+ return HALSIM_GetNumNotifiers();
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/SimulatorJNI.cpp b/hal/src/main/native/sim/jni/SimulatorJNI.cpp
index bba8f01..9226f91 100644
--- a/hal/src/main/native/sim/jni/SimulatorJNI.cpp
+++ b/hal/src/main/native/sim/jni/SimulatorJNI.cpp
@@ -144,6 +144,54 @@
/*
* Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI
+ * Method: pauseTiming
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_pauseTiming
+ (JNIEnv*, jclass)
+{
+ HALSIM_PauseTiming();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI
+ * Method: resumeTiming
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_resumeTiming
+ (JNIEnv*, jclass)
+{
+ HALSIM_ResumeTiming();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI
+ * Method: isTimingPaused
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_isTimingPaused
+ (JNIEnv*, jclass)
+{
+ return HALSIM_IsTimingPaused();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI
+ * Method: stepTiming
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_stepTiming
+ (JNIEnv*, jclass, jlong delta)
+{
+ HALSIM_StepTiming(delta);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI
* Method: resetHandles
* Signature: ()V
*/
diff --git a/hal/src/main/native/sim/mockdata/AddressableLEDData.cpp b/hal/src/main/native/sim/mockdata/AddressableLEDData.cpp
new file mode 100644
index 0000000..5b44eaf
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/AddressableLEDData.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include <algorithm>
+#include <cstring>
+
+#include "../PortsInternal.h"
+#include "AddressableLEDDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeAddressableLEDData() {
+ static AddressableLEDData sad[kNumAddressableLEDs];
+ ::hal::SimAddressableLEDData = sad;
+}
+} // namespace init
+} // namespace hal
+
+AddressableLEDData* hal::SimAddressableLEDData;
+
+void AddressableLEDData::ResetData() {
+ initialized.Reset(false);
+ outputPort.Reset(-1);
+ length.Reset(1);
+ running.Reset(false);
+ data.Reset();
+}
+
+void AddressableLEDData::SetData(const HAL_AddressableLEDData* d, int32_t len) {
+ len = (std::min)(HAL_kAddressableLEDMaxLength, len);
+ {
+ std::scoped_lock lock(m_dataMutex);
+ std::memcpy(m_data, d, len * sizeof(d[0]));
+ }
+ data(reinterpret_cast<const uint8_t*>(d), len * sizeof(d[0]));
+}
+
+int32_t AddressableLEDData::GetData(HAL_AddressableLEDData* d) {
+ std::scoped_lock lock(m_dataMutex);
+ int32_t len = length;
+ if (d) std::memcpy(d, m_data, len * sizeof(d[0]));
+ return len;
+}
+
+extern "C" {
+void HALSIM_ResetAddressableLEDData(int32_t index) {
+ SimAddressableLEDData[index].ResetData();
+}
+
+int32_t HALSIM_GetAddressableLEDData(int32_t index,
+ struct HAL_AddressableLEDData* data) {
+ return SimAddressableLEDData[index].GetData(data);
+}
+
+void HALSIM_SetAddressableLEDData(int32_t index,
+ const struct HAL_AddressableLEDData* data,
+ int32_t length) {
+ SimAddressableLEDData[index].SetData(data, length);
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, AddressableLED##CAPINAME, \
+ SimAddressableLEDData, LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+DEFINE_CAPI(int32_t, OutputPort, outputPort)
+DEFINE_CAPI(int32_t, Length, length)
+DEFINE_CAPI(HAL_Bool, Running, running)
+
+#undef DEFINE_CAPI
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMCALLBACKREGISTRY_DEFINE_CAPI(TYPE, HALSIM, AddressableLED##CAPINAME, \
+ SimAddressableLEDData, LOWERNAME)
+
+DEFINE_CAPI(HAL_ConstBufferCallback, Data, data)
+
+#define REGISTER(NAME) \
+ SimAddressableLEDData[index].NAME.RegisterCallback(callback, param, \
+ initialNotify)
+
+void HALSIM_RegisterAddressableLEDAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify) {
+ REGISTER(initialized);
+ REGISTER(outputPort);
+ REGISTER(length);
+ REGISTER(running);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/AddressableLEDDataInternal.h b/hal/src/main/native/sim/mockdata/AddressableLEDDataInternal.h
new file mode 100644
index 0000000..9d6e215
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/AddressableLEDDataInternal.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <vector>
+
+#include <wpi/spinlock.h>
+
+#include "mockdata/AddressableLEDData.h"
+#include "mockdata/SimCallbackRegistry.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class AddressableLEDData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+ HAL_SIMDATAVALUE_DEFINE_NAME(OutputPort)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Length)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Running)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Data)
+
+ wpi::recursive_spinlock m_dataMutex;
+ HAL_AddressableLEDData m_data[HAL_kAddressableLEDMaxLength];
+
+ public:
+ void SetData(const HAL_AddressableLEDData* d, int32_t len);
+ int32_t GetData(HAL_AddressableLEDData* d);
+
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+ false};
+ SimDataValue<int32_t, HAL_MakeInt, GetOutputPortName> outputPort{-1};
+ SimDataValue<int32_t, HAL_MakeInt, GetLengthName> length{1};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetRunningName> running{false};
+ SimCallbackRegistry<HAL_ConstBufferCallback, GetDataName> data;
+
+ void ResetData();
+};
+extern AddressableLEDData* SimAddressableLEDData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/DutyCycleData.cpp b/hal/src/main/native/sim/mockdata/DutyCycleData.cpp
new file mode 100644
index 0000000..04806e0
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/DutyCycleData.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "../PortsInternal.h"
+#include "DutyCycleDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeDutyCycleData() {
+ static DutyCycleData sed[kNumDutyCycles];
+ ::hal::SimDutyCycleData = sed;
+}
+} // namespace init
+} // namespace hal
+
+DutyCycleData* hal::SimDutyCycleData;
+
+void DutyCycleData::ResetData() {
+ digitalChannel = 0;
+ initialized.Reset(false);
+ simDevice = 0;
+ frequency.Reset(0);
+ output.Reset(0);
+}
+
+extern "C" {
+void HALSIM_ResetDutyCycleData(int32_t index) {
+ SimDutyCycleData[index].ResetData();
+}
+int32_t HALSIM_GetDutyCycleDigitalChannel(int32_t index) {
+ return SimDutyCycleData[index].digitalChannel;
+}
+
+HAL_SimDeviceHandle HALSIM_GetDutyCycleSimDevice(int32_t index) {
+ return SimDutyCycleData[index].simDevice;
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, DutyCycle##CAPINAME, \
+ SimDutyCycleData, LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+DEFINE_CAPI(int32_t, Frequency, frequency)
+DEFINE_CAPI(double, Output, output)
+
+#define REGISTER(NAME) \
+ SimDutyCycleData[index].NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterDutyCycleAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify) {
+ REGISTER(initialized);
+ REGISTER(frequency);
+ REGISTER(output);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/DutyCycleDataInternal.h b/hal/src/main/native/sim/mockdata/DutyCycleDataInternal.h
new file mode 100644
index 0000000..2f98b07
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/DutyCycleDataInternal.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <atomic>
+#include <limits>
+
+#include "mockdata/DutyCycleData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class DutyCycleData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Output)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Frequency)
+
+ public:
+ std::atomic<int32_t> digitalChannel{0};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+ false};
+ std::atomic<HAL_SimDeviceHandle> simDevice;
+ SimDataValue<int32_t, HAL_MakeInt, GetFrequencyName> frequency{0};
+ SimDataValue<double, HAL_MakeDouble, GetOutputName> output{0};
+
+ virtual void ResetData();
+};
+extern DutyCycleData* SimDutyCycleData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/PCMData.cpp b/hal/src/main/native/sim/mockdata/PCMData.cpp
index df68e04..6193b05 100644
--- a/hal/src/main/native/sim/mockdata/PCMData.cpp
+++ b/hal/src/main/native/sim/mockdata/PCMData.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -49,6 +49,23 @@
DEFINE_CAPI(HAL_Bool, PressureSwitch, pressureSwitch)
DEFINE_CAPI(double, CompressorCurrent, compressorCurrent)
+void HALSIM_GetPCMAllSolenoids(int32_t index, uint8_t* values) {
+ auto& data = SimPCMData[index].solenoidOutput;
+ uint8_t ret = 0;
+ for (int i = 0; i < kNumSolenoidChannels; i++) {
+ ret |= (data[i] << i);
+ }
+ *values = ret;
+}
+
+void HALSIM_SetPCMAllSolenoids(int32_t index, uint8_t values) {
+ auto& data = SimPCMData[index].solenoidOutput;
+ for (int i = 0; i < kNumSolenoidChannels; i++) {
+ data[i] = (values & 0x1) != 0;
+ values >>= 1;
+ }
+}
+
#define REGISTER(NAME) \
SimPCMData[index].NAME.RegisterCallback(callback, param, initialNotify)
diff --git a/hal/src/main/native/sim/mockdata/PDPData.cpp b/hal/src/main/native/sim/mockdata/PDPData.cpp
index 9c6143e..1c150bb 100644
--- a/hal/src/main/native/sim/mockdata/PDPData.cpp
+++ b/hal/src/main/native/sim/mockdata/PDPData.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -42,6 +42,20 @@
HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(double, HALSIM, PDPCurrent, SimPDPData,
current)
+void HALSIM_GetPDPAllCurrents(int32_t index, double* currents) {
+ auto& data = SimPDPData[index].current;
+ for (int i = 0; i < kNumPDPChannels; i++) {
+ currents[i] = data[i];
+ }
+}
+
+void HALSIM_SetPDPAllCurrents(int32_t index, const double* currents) {
+ auto& data = SimPDPData[index].current;
+ for (int i = 0; i < kNumPDPChannels; i++) {
+ data[i] = currents[i];
+ }
+}
+
#define REGISTER(NAME) \
SimPDPData[index].NAME.RegisterCallback(callback, param, initialNotify)
diff --git a/hal/src/test/java/edu/wpi/first/hal/sim/SimDeviceSimTest.java b/hal/src/test/java/edu/wpi/first/hal/sim/SimDeviceSimTest.java
new file mode 100644
index 0000000..29452a0
--- /dev/null
+++ b/hal/src/test/java/edu/wpi/first/hal/sim/SimDeviceSimTest.java
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* 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.hal.sim;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class SimDeviceSimTest {
+ @Test
+ void testBasic() {
+ SimDevice dev = SimDevice.create("test");
+ SimBoolean devBool = dev.createBoolean("bool", false, false);
+
+ SimDeviceSim sim = new SimDeviceSim("test");
+ SimBoolean simBool = sim.getBoolean("bool");
+
+ assertFalse(simBool.get());
+ simBool.set(true);
+ assertTrue(devBool.get());
+ }
+}
diff --git a/hal/src/test/native/cpp/mockdata/DriverStationDataTests.cpp b/hal/src/test/native/cpp/mockdata/DriverStationDataTests.cpp
index 35c0d54..5505c06 100644
--- a/hal/src/test/native/cpp/mockdata/DriverStationDataTests.cpp
+++ b/hal/src/test/native/cpp/mockdata/DriverStationDataTests.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -50,7 +50,7 @@
int joystickUnderTest = 4;
set_axes.count = 5;
for (int i = 0; i < set_axes.count; ++i) {
- set_axes.axes[i] = i * .125;
+ set_axes.axes[i] = i * 0.125;
}
set_povs.count = 3;
diff --git a/hal/src/test/native/cpp/sim/SimDeviceSimTest.cpp b/hal/src/test/native/cpp/sim/SimDeviceSimTest.cpp
new file mode 100644
index 0000000..ba0646d
--- /dev/null
+++ b/hal/src/test/native/cpp/sim/SimDeviceSimTest.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include <wpi/StringRef.h>
+
+#include "gtest/gtest.h"
+#include "hal/SimDevice.h"
+#include "simulation/SimDeviceSim.h"
+
+using namespace frc::sim;
+
+namespace hal {
+
+TEST(SimDeviceSimTests, TestBasic) {
+ SimDevice dev{"test"};
+ SimBoolean devBool = dev.CreateBoolean("bool", false, false);
+
+ SimDeviceSim sim{"test"};
+ SimBoolean simBool = sim.GetBoolean("bool");
+ EXPECT_FALSE(simBool.Get());
+ simBool.Set(true);
+ EXPECT_TRUE(devBool.Get());
+}
+
+TEST(SimDeviceSimTests, TestEnumerateDevices) {
+ SimDevice dev{"test"};
+
+ bool foundit = false;
+ SimDeviceSim::EnumerateDevices(
+ "te", [&](const char* name, HAL_SimDeviceHandle handle) {
+ if (wpi::StringRef(name) == "test") foundit = true;
+ });
+ EXPECT_TRUE(foundit);
+}
+
+} // namespace hal
diff --git a/myRobot/build.gradle b/myRobot/build.gradle
index 51dcbdd..d2f1194 100644
--- a/myRobot/build.gradle
+++ b/myRobot/build.gradle
@@ -1,3 +1,6 @@
+import jaci.gradle.toolchains.*
+import jaci.gradle.nativedeps.*
+
plugins {
id 'java'
id 'application'
@@ -6,6 +9,7 @@
}
apply plugin: 'edu.wpi.first.NativeUtils'
+apply plugin: 'jaci.gradle.EmbeddedTools'
apply from: '../shared/config.gradle'
@@ -17,15 +21,93 @@
skipDev = true
}
-ext {
- chipObjectComponents = ['myRobotCpp', 'myRobotCppStatic']
- netCommComponents = ['myRobotCpp', 'myRobotCppStatic']
- useNiJava = true
+apply from: "${rootDir}/shared/opencv.gradle"
+
+deploy {
+ targets {
+ target('roborio') {
+ directory = '/home/admin'
+ maxChannels = 4
+ locations {
+ ssh {
+ address = "172.22.11.2"
+ user = 'admin'
+ password = ''
+ ipv6 = false
+ }
+ }
+ }
+ }
+ artifacts {
+ all {
+ targets << 'roborio'
+ predeploy << { ctx ->
+ ctx.execute('/usr/local/frc/bin/frcKillRobot.sh -t')
+ }
+ postdeploy << { ctx ->
+ ctx.execute("sync")
+ ctx.execute("ldconfig")
+ }
+ }
+
+ artifact('jre', JREArtifact) {
+ jreDependency = 'edu.wpi.first.jdk:roborio-2020:11.0.4u10-2'
+ }
+
+ javaArtifact('myRobotJava') {
+ jar = 'shadowJar'
+ postdeploy << { ctx ->
+ ctx.execute("echo '/usr/local/frc/JRE/bin/java -XX:+UseConcMarkSweepGC -Djava.library.path=/usr/local/frc/third-party/lib -Djava.lang.invoke.stringConcat=BC_SB -jar /home/admin/myRobot-all.jar' > /home/admin/myRobotJavaRun")
+ ctx.execute("chmod +x /home/admin/myRobotJavaRun; chown lvuser /home/admin/myRobotJavaRun")
+ }
+ }
+
+ nativeArtifact('myRobotCpp') {
+ component = 'myRobotCpp'
+ targetPlatform = nativeUtils.wpi.platforms.roborio
+ libraryDirectory = '/usr/local/frc/third-party/lib'
+ buildType = 'debug'
+ postdeploy << { ctx ->
+ ctx.execute('chmod +x myRobotCpp')
+ }
+
+ }
+
+ nativeArtifact('myRobotCppStatic') {
+ component = 'myRobotCppStatic'
+ targetPlatform = nativeUtils.wpi.platforms.roborio
+ buildType = 'debug'
+
+ postdeploy << { ctx ->
+ ctx.execute('chmod +x myRobotCppStatic')
+ }
+ }
+ }
}
-apply from: "${rootDir}/shared/nilibraries.gradle"
+tasks.register('deployJava') {
+ try {
+ dependsOn tasks.named('deployJreRoborio')
+ dependsOn tasks.named('deployMyRobotJavaRoborio')
+ dependsOn tasks.named('deployMyRobotCppLibrariesRoborio')
+ } catch (ignored) {
+ }
+}
-apply from: "${rootDir}/shared/opencv.gradle"
+tasks.register('deployShared') {
+ try {
+ dependsOn tasks.named('deployMyRobotCppLibrariesRoborio')
+ dependsOn tasks.named('deployMyRobotCppRoborio')
+ } catch (ignored) {
+ }
+}
+
+tasks.register('deployStatic') {
+ try {
+ dependsOn tasks.named('deployMyRobotCppStaticRoborio')
+ } catch (ignored) {
+ }
+}
mainClassName = 'Main'
@@ -36,23 +118,20 @@
}
dependencies {
- compile project(':wpilibj')
- compile project(':hal')
- compile project(':wpiutil')
- compile project(':ntcore')
- compile project(':cscore')
- compile project(':cameraserver')
-}
-
-jar {
- manifest { attributes 'Robot-Class': 'MyRobot' }
+ implementation project(':wpilibj')
+ implementation project(':hal')
+ implementation project(':wpiutil')
+ implementation project(':ntcore')
+ implementation project(':cscore')
+ implementation project(':cameraserver')
+ implementation project(':wpilibOldCommands')
+ implementation project(':wpilibNewCommands')
}
model {
components {
myRobotCpp(NativeExecutableSpec) {
targetBuildTypes 'debug'
- baseName = 'FRCUserProgram'
sources {
cpp {
source {
@@ -66,6 +145,8 @@
}
}
binaries.all { binary ->
+ lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
+ lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
lib project: ':cscore', library: 'cscore', linkage: 'shared'
@@ -75,11 +156,14 @@
project(':hal').addHalJniDependency(binary)
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+ if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
}
}
myRobotCppStatic(NativeExecutableSpec) {
targetBuildTypes 'debug'
- baseName = 'FRCUserProgram'
+ nativeUtils.excludeBinariesFromStrip(it)
sources {
cpp {
source {
@@ -93,12 +177,17 @@
}
}
binaries.all { binary ->
+ lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'static'
+ lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'static'
lib project: ':wpilibc', library: 'wpilibc', linkage: 'static'
lib project: ':ntcore', library: 'ntcore', linkage: 'static'
lib project: ':cscore', library: 'cscore', linkage: 'static'
project(':hal').addHalDependency(binary, 'static')
lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
lib project: ':cameraserver', library: 'cameraserver', linkage: 'static'
+ if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
}
}
}
diff --git a/myRobot/src/main/native/cpp/MyRobot.cpp b/myRobot/src/main/native/cpp/MyRobot.cpp
index ff066d8..79c6e9f 100644
--- a/myRobot/src/main/native/cpp/MyRobot.cpp
+++ b/myRobot/src/main/native/cpp/MyRobot.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/settings.gradle b/settings.gradle
index 1c1fa29..084cf5e 100644
--- a/settings.gradle
+++ b/settings.gradle
@@ -5,6 +5,10 @@
}
}
+plugins {
+ id "com.gradle.enterprise" version "3.0"
+}
+
// Set the flag to tell gradle to ignore unresolved headers
// Libraries like eigen and opencv use macro includes, which
// Gradle doesn't properly ignore, and completely disables
@@ -33,5 +37,7 @@
include 'simulation:halsim_gui'
include 'cameraserver'
include 'cameraserver:multiCameraServer'
+include 'wpilibOldCommands'
+include 'wpilibNewCommands'
include 'myRobot'
include 'docs'
diff --git a/shared/config.gradle b/shared/config.gradle
index 0173097..20ec083 100644
--- a/shared/config.gradle
+++ b/shared/config.gradle
@@ -8,7 +8,7 @@
wpi {
configureDependencies {
wpiVersion = "-1"
- niLibVersion = "2020.5.1"
+ niLibVersion = "2020.9.1"
opencvVersion = "3.4.7-2"
googleTestVersion = "1.9.0-3-437e100"
imguiVersion = "1.72b-2"
@@ -90,9 +90,9 @@
configMap.each { key, value ->
def task = project.tasks.create(base + "-${key}", type) {
description = 'Creates component archive for platform ' + key
- destinationDir = outputsFolder
+ destinationDirectory = outputsFolder
classifier = key
- baseName = '_M_' + base
+ archiveBaseName = '_M_' + base
duplicatesStrategy = 'exclude'
from(licenseFile) {
diff --git a/shared/examplecheck.gradle b/shared/examplecheck.gradle
index 64a83c7..4e7bdc2 100644
--- a/shared/examplecheck.gradle
+++ b/shared/examplecheck.gradle
@@ -1,5 +1,5 @@
-def fileCheck = { file, folder ->
- def folderNames = new groovy.json.JsonSlurper().parseText(file.text).collect { it.foldername }
+def fileCheck = { parsedJson, folder ->
+ def folderNames = parsedJson.collect { it.foldername }
def folders = []
folder.eachDir {
folders << it.name
@@ -26,19 +26,57 @@
task checkTemplates(type: Task) {
doLast {
- fileCheck(templateFile, templateDirectory)
+ def parsedJson = new groovy.json.JsonSlurper().parseText(templateFile.text)
+ fileCheck(parsedJson, templateDirectory)
+ parsedJson.each {
+ assert it.name != null
+ assert it.description != null
+ assert it.tags != null
+ assert it.foldername != null
+ assert it.gradlebase != null
+ assert it.commandversion != null
+ if (it.gradlebase == 'java') {
+ assert it.mainclass != null
+ }
+ }
}
}
task checkExamples(type: Task) {
doLast {
- fileCheck(exampleFile, exampleDirectory)
+ def parsedJson = new groovy.json.JsonSlurper().parseText(exampleFile.text)
+ fileCheck(parsedJson, exampleDirectory)
+ parsedJson.each {
+ assert it.name != null
+ assert it.description != null
+ assert it.tags != null
+ assert it.foldername != null
+ assert it.gradlebase != null
+ assert it.commandversion != null
+ if (it.gradlebase == 'java') {
+ assert it.mainclass != null
+ }
+ }
}
}
task checkCommands(type: Task) {
doLast {
- fileCheck(commandFile, commandDirectory)
+ def parsedJson = new groovy.json.JsonSlurper().parseText(commandFile.text)
+ fileCheck(parsedJson, commandDirectory)
+ parsedJson.each {
+ assert it.name != null
+ assert it.description != null
+ assert it.tags != null
+ assert it.foldername != null
+ assert it.replacename != null
+ if (project.isCppCommands) {
+ assert it.headers != null
+ assert !it.headers.isEmpty()
+ assert it.source != null
+ assert !it.source.isEmpty()
+ }
+ }
}
}
diff --git a/shared/java/javacommon.gradle b/shared/java/javacommon.gradle
index 4ade369..d83ead2 100644
--- a/shared/java/javacommon.gradle
+++ b/shared/java/javacommon.gradle
@@ -1,5 +1,5 @@
apply plugin: 'maven-publish'
-apply plugin: 'java'
+apply plugin: 'java-library'
//apply plugin: 'net.ltgt.errorprone'
apply plugin: 'jacoco'
@@ -20,21 +20,21 @@
}
task outputJar(type: Jar, dependsOn: classes) {
- baseName javaBaseName
- destinationDir outputsFolder
+ archiveBaseName = javaBaseName
+ destinationDirectory = outputsFolder
from sourceSets.main.output
}
task outputSourcesJar(type: Jar, dependsOn: classes) {
- baseName javaBaseName
- destinationDir outputsFolder
+ archiveBaseName = javaBaseName
+ destinationDirectory = outputsFolder
classifier = 'sources'
from sourceSets.main.allSource
}
task outputJavadocJar(type: Jar, dependsOn: javadoc) {
- baseName javaBaseName
- destinationDir outputsFolder
+ archiveBaseName = javaBaseName
+ destinationDirectory = outputsFolder
classifier = 'javadoc'
from javadoc.destinationDir
}
@@ -104,7 +104,7 @@
testImplementation 'org.junit.jupiter:junit-jupiter-params:5.4.2'
testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.4.2'
- devCompile sourceSets.main.output
+ devImplementation sourceSets.main.output
//errorprone 'com.google.errorprone:error_prone_core:2.3.2-SNAPSHOT'
//errorproneJavac 'com.google.errorprone:error_prone_core:2.3.1'
diff --git a/shared/java/javastyle.gradle b/shared/java/javastyle.gradle
index 6e4e261..eec3796 100644
--- a/shared/java/javastyle.gradle
+++ b/shared/java/javastyle.gradle
@@ -3,8 +3,8 @@
checkstyle {
toolVersion = "8.12"
- configDir = file("${project.rootDir}/styleguide")
- config = resources.text.fromFile(new File(configDir, "checkstyle.xml"))
+ configDirectory = file("${project.rootDir}/styleguide")
+ config = resources.text.fromFile(new File(configDirectory.get().getAsFile(), "checkstyle.xml"))
}
if (!project.hasProperty('skipPMD')) {
diff --git a/shared/javacpp/publish.gradle b/shared/javacpp/publish.gradle
index 1dafcbb..5002def 100644
--- a/shared/javacpp/publish.gradle
+++ b/shared/javacpp/publish.gradle
@@ -9,8 +9,8 @@
def licenseFile = file("$rootDir/license.txt")
task cppSourcesZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = zipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = zipBaseName
classifier = "sources"
from(licenseFile) {
@@ -23,8 +23,8 @@
}
task cppHeadersZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = zipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = zipBaseName
classifier = "headers"
from(licenseFile) {
diff --git a/shared/jni/publish.gradle b/shared/jni/publish.gradle
index e50f87e..765302a 100644
--- a/shared/jni/publish.gradle
+++ b/shared/jni/publish.gradle
@@ -11,8 +11,8 @@
def licenseFile = file("$rootDir/license.txt")
task cppSourcesZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = zipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = zipBaseName
classifier = "sources"
duplicatesStrategy = 'exclude'
@@ -41,8 +41,8 @@
}
task cppHeadersZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = zipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = zipBaseName
classifier = "headers"
from(licenseFile) {
diff --git a/shared/jni/setupBuild.gradle b/shared/jni/setupBuild.gradle
index 35f4cf2..7622d0e 100644
--- a/shared/jni/setupBuild.gradle
+++ b/shared/jni/setupBuild.gradle
@@ -17,8 +17,8 @@
dependencies {
if (!project.hasProperty('noWpiutil')) {
- compile project(':wpiutil')
- devCompile project(':wpiutil')
+ implementation project(':wpiutil')
+ devImplementation project(':wpiutil')
}
}
@@ -32,18 +32,6 @@
apply from: "${rootDir}/shared/googletest.gradle"
-if (project.hasProperty('niLibraries')) {
- ext {
- chipObjectComponents = ["$nativeName".toString(), "${nativeName}Dev".toString(), "${nativeName}Base".toString(),
- "${nativeName}JNI".toString(), "${nativeName}JNIShared".toString(), "${nativeName}Test".toString()]
- netCommComponents = ["$nativeName".toString(), "${nativeName}Dev".toString(), "${nativeName}Base".toString(),
- "${nativeName}JNI".toString(), "${nativeName}JNIShared".toString(), "${nativeName}Test".toString()]
- useNiJava = true
- }
-
- apply from: "${rootDir}/shared/nilibraries.gradle"
-}
-
model {
components {
"${nativeName}Base"(NativeLibrarySpec) {
@@ -211,6 +199,9 @@
if (!project.hasProperty('noWpiutil')) {
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
}
+ if (nativeName == 'hal' && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
}
}
}
@@ -243,6 +234,9 @@
lib library: nativeName, linkage: 'shared'
if (!project.hasProperty('noWpiutil')) {
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+ if (nativeName == 'hal' && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
}
}
}
diff --git a/shared/nilibraries.gradle b/shared/nilibraries.gradle
deleted file mode 100644
index efb9cfc..0000000
--- a/shared/nilibraries.gradle
+++ /dev/null
@@ -1,10 +0,0 @@
-model {
- binaries {
- all {
- if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
- nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared')
- }
-
- }
- }
-}
diff --git a/shared/opencv.gradle b/shared/opencv.gradle
index 7a1dd0f..ca3764d 100644
--- a/shared/opencv.gradle
+++ b/shared/opencv.gradle
@@ -22,9 +22,9 @@
if (project.hasProperty('useJava') && project.useJava) {
dependencies {
- compile "edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:${opencvVersion}"
+ implementation "edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:${opencvVersion}"
if (!project.hasProperty('skipDev') || !project.skipDev) {
- devCompile "edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:${opencvVersion}"
+ devImplementation "edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:${opencvVersion}"
}
if (project.hasProperty('useDocumentation') && project.useDocumentation) {
javaSource "edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:${opencvVersion}:sources"
diff --git a/shared/plugins/publish.gradle b/shared/plugins/publish.gradle
index 6f318b7..ec160ed 100644
--- a/shared/plugins/publish.gradle
+++ b/shared/plugins/publish.gradle
@@ -7,8 +7,8 @@
def outputsFolder = file("$project.buildDir/outputs")
task cppSourcesZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = zipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = zipBaseName
classifier = "sources"
from(licenseFile) {
@@ -21,8 +21,8 @@
}
task cppHeadersZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = zipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = zipBaseName
classifier = "headers"
from(licenseFile) {
diff --git a/shared/plugins/setupBuild.gradle b/shared/plugins/setupBuild.gradle
index 4feb8a9..af9b4c4 100644
--- a/shared/plugins/setupBuild.gradle
+++ b/shared/plugins/setupBuild.gradle
@@ -2,14 +2,6 @@
apply plugin: 'edu.wpi.first.NativeUtils'
apply plugin: ExtraTasks
-ext {
- chipObjectComponents = ["$pluginName".toString(), "${pluginName}Dev".toString(), "${pluginName}Test".toString()]
- netCommComponents = ["$pluginName".toString(), "${pluginName}Dev".toString(), "${pluginName}Test".toString()]
- useNiJava = false
-}
-
-apply from: "${rootDir}/shared/nilibraries.gradle"
-
if (!project.hasProperty('onlylinuxathena')) {
ext.skiplinuxathena = true
apply from: "${rootDir}/shared/config.gradle"
@@ -64,6 +56,9 @@
lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
}
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+ if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
} else {
it.buildable = false
}
diff --git a/simulation/gz_msgs/build.gradle b/simulation/gz_msgs/build.gradle
index 44f9692..353f4ad 100644
--- a/simulation/gz_msgs/build.gradle
+++ b/simulation/gz_msgs/build.gradle
@@ -1,95 +1,95 @@
-plugins {
- id 'cpp'
- id 'java'
- id 'com.google.protobuf' version '0.8.8'
- id 'edu.wpi.first.NativeUtils'
-}
-
-description = "A C++ and Java library to pass FRC Simulation Messages in and out of Gazebo."
-
-/* The simulation does not run on real hardware; so we always skip Athena */
-ext.skiplinuxathena = true
-ext.skiplinuxraspbian = true
-apply from: "${rootDir}/shared/config.gradle"
-
-/* Use a sort of poor man's autoconf to find the protobuf development
- files; on Debian, those are supplied by libprotobuf-dev.
-
- This should get skipped on Windows.
-
- TODO: Add Windows support for the simulation code */
-
-def protobuf_version = ""
-try {
- protobuf_version = "pkg-config --modversion protobuf".execute().text.trim()
- println "Protobuf version is [${protobuf_version}]"
-} catch(Exception ex) {
-}
-
-if (!protobuf_version?.trim()) {
- println "Protobuf is not available. (pkg-config --modversion protobuf failed)"
- protobuf_version = "+"
- if (project.hasProperty("makeSim")) {
- /* Force the build even though we did not find protobuf. */
- println "makeSim set. Forcing build - failure likely."
- }
- else {
- ext.skip_gz_msgs = true
- println "Skipping gz_msgs."
- }
-}
-
-tasks.whenTaskAdded { task ->
- task.onlyIf { !project.hasProperty('skip_gz_msgs') }
-}
-
-dependencies {
- compile "com.google.protobuf:protobuf-java:${protobuf_version}"
- compile "com.google.protobuf:protoc:${protobuf_version}"
-}
-
-/* There is a nice gradle plugin for protobuf, and the protoc tool
- is included; using it simplifies our build process.
- The trick is that we have to use the same version as the system
- copy of libprotobuf-dev */
-protobuf {
- protoc {
- artifact = "com.google.protobuf:protoc:${protobuf_version}"
- }
-
- generatedFilesBaseDir = "$buildDir/generated"
- generateProtoTasks {
- all().each { task ->
- task.builtins {
- cpp {
- outputSubDir = 'simulation/gz_msgs'
- }
- }
- }
- }
-}
-
-model {
- components {
- gz_msgs(NativeLibrarySpec) {
- sources {
- cpp {
- source {
- srcDir "$buildDir/generated/main/simulation/gz_msgs"
- builtBy tasks.generateProto
- }
- exportedHeaders {
- srcDir "src/include"
- srcDir "$buildDir/generated/main"
- }
- }
- }
- /* We must compile with -fPIC to link the static library into an so */
- binaries {
- all {
- cppCompiler.args "-fPIC"
- }
- }
- }
- }
-}
+plugins {
+ id 'cpp'
+ id 'java'
+ id 'com.google.protobuf' version '0.8.8'
+ id 'edu.wpi.first.NativeUtils'
+}
+
+description = "A C++ and Java library to pass FRC Simulation Messages in and out of Gazebo."
+
+/* The simulation does not run on real hardware; so we always skip Athena */
+ext.skiplinuxathena = true
+ext.skiplinuxraspbian = true
+apply from: "${rootDir}/shared/config.gradle"
+
+/* Use a sort of poor man's autoconf to find the protobuf development
+ files; on Debian, those are supplied by libprotobuf-dev.
+
+ This should get skipped on Windows.
+
+ TODO: Add Windows support for the simulation code */
+
+def protobuf_version = ""
+try {
+ protobuf_version = "pkg-config --modversion protobuf".execute().text.trim()
+ println "Protobuf version is [${protobuf_version}]"
+} catch(Exception ex) {
+}
+
+if (!protobuf_version?.trim()) {
+ println "Protobuf is not available. (pkg-config --modversion protobuf failed)"
+ protobuf_version = "+"
+ if (project.hasProperty("makeSim")) {
+ /* Force the build even though we did not find protobuf. */
+ println "makeSim set. Forcing build - failure likely."
+ }
+ else {
+ ext.skip_gz_msgs = true
+ println "Skipping gz_msgs."
+ }
+}
+
+tasks.whenTaskAdded { task ->
+ task.onlyIf { !project.hasProperty('skip_gz_msgs') }
+}
+
+dependencies {
+ implementation "com.google.protobuf:protobuf-java:${protobuf_version}"
+ implementation "com.google.protobuf:protoc:${protobuf_version}"
+}
+
+/* There is a nice gradle plugin for protobuf, and the protoc tool
+ is included; using it simplifies our build process.
+ The trick is that we have to use the same version as the system
+ copy of libprotobuf-dev */
+protobuf {
+ protoc {
+ artifact = "com.google.protobuf:protoc:${protobuf_version}"
+ }
+
+ generatedFilesBaseDir = "$buildDir/generated"
+ generateProtoTasks {
+ all().each { task ->
+ task.builtins {
+ cpp {
+ outputSubDir = 'simulation/gz_msgs'
+ }
+ }
+ }
+ }
+}
+
+model {
+ components {
+ gz_msgs(NativeLibrarySpec) {
+ sources {
+ cpp {
+ source {
+ srcDir "$buildDir/generated/main/simulation/gz_msgs"
+ builtBy tasks.generateProto
+ }
+ exportedHeaders {
+ srcDir "src/include"
+ srcDir "$buildDir/generated/main"
+ }
+ }
+ }
+ /* We must compile with -fPIC to link the static library into an so */
+ binaries {
+ all {
+ cppCompiler.args "-fPIC"
+ }
+ }
+ }
+ }
+}
diff --git a/simulation/halsim_ds_socket/build.gradle b/simulation/halsim_ds_socket/build.gradle
index 46e48f4..440e5d9 100644
--- a/simulation/halsim_ds_socket/build.gradle
+++ b/simulation/halsim_ds_socket/build.gradle
@@ -48,6 +48,9 @@
project(':hal').addHalDependency(it, 'shared')
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
lib library: pluginName, linkage: 'shared'
+ if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
}
}
}
diff --git a/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp b/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp
index aba88fb..2c1e83a 100644
--- a/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp
+++ b/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -7,7 +7,8 @@
#include <thread>
-#include <hal/HAL.h>
+#include <hal/DriverStation.h>
+#include <hal/HALBase.h>
#include <wpi/Format.h>
#include <wpi/raw_ostream.h>
diff --git a/simulation/halsim_ds_socket/src/test/native/cpp/main.cpp b/simulation/halsim_ds_socket/src/test/native/cpp/main.cpp
index fd8f022..c6b6c58 100644
--- a/simulation/halsim_ds_socket/src/test/native/cpp/main.cpp
+++ b/simulation/halsim_ds_socket/src/test/native/cpp/main.cpp
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include <hal/HAL.h>
+#include <hal/HALBase.h>
#include "gtest/gtest.h"
diff --git a/simulation/halsim_gui/build.gradle b/simulation/halsim_gui/build.gradle
index 5ac68ea..5df5302 100644
--- a/simulation/halsim_gui/build.gradle
+++ b/simulation/halsim_gui/build.gradle
@@ -1,40 +1,40 @@
-if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxraspbian') && !project.hasProperty('onlylinuxaarch64bionic')) {
-
- description = "A plugin that creates a simulation gui"
-
- ext {
- includeWpiutil = true
- pluginName = 'halsim_gui'
- }
-
- apply plugin: 'google-test-test-suite'
-
-
- ext {
- staticGtestConfigs = [:]
- }
-
- staticGtestConfigs["${pluginName}Test"] = []
- apply from: "${rootDir}/shared/googletest.gradle"
-
- apply from: "${rootDir}/shared/plugins/setupBuild.gradle"
-
- model {
- binaries {
- all {
- nativeUtils.useRequiredLibrary(it, 'imgui_static')
- if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name == nativeUtils.wpi.platforms.raspbian || it.targetPlatform.name == nativeUtils.wpi.platforms.aarch64bionic) {
- it.buildable = false
- return
- }
- if (it.targetPlatform.operatingSystem.isWindows()) {
- it.linker.args << 'Gdi32.lib' << 'Shell32.lib'
- } else if (it.targetPlatform.operatingSystem.isMacOsX()) {
- it.linker.args << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo'
- } else {
- it.linker.args << '-lX11' << '-lvulkan'
- }
- }
- }
- }
-}
+if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxraspbian') && !project.hasProperty('onlylinuxaarch64bionic')) {
+
+ description = "A plugin that creates a simulation gui"
+
+ ext {
+ includeWpiutil = true
+ pluginName = 'halsim_gui'
+ }
+
+ apply plugin: 'google-test-test-suite'
+
+
+ ext {
+ staticGtestConfigs = [:]
+ }
+
+ staticGtestConfigs["${pluginName}Test"] = []
+ apply from: "${rootDir}/shared/googletest.gradle"
+
+ apply from: "${rootDir}/shared/plugins/setupBuild.gradle"
+
+ model {
+ binaries {
+ all {
+ nativeUtils.useRequiredLibrary(it, 'imgui_static')
+ if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name == nativeUtils.wpi.platforms.raspbian || it.targetPlatform.name == nativeUtils.wpi.platforms.aarch64bionic) {
+ it.buildable = false
+ return
+ }
+ if (it.targetPlatform.operatingSystem.isWindows()) {
+ it.linker.args << 'Gdi32.lib' << 'Shell32.lib'
+ } else if (it.targetPlatform.operatingSystem.isMacOsX()) {
+ it.linker.args << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo'
+ } else {
+ it.linker.args << '-lX11' << '-lvulkan'
+ }
+ }
+ }
+ }
+}
diff --git a/simulation/halsim_gui/src/main/native/cpp/AddressableLEDGui.cpp b/simulation/halsim_gui/src/main/native/cpp/AddressableLEDGui.cpp
new file mode 100644
index 0000000..606eac5
--- /dev/null
+++ b/simulation/halsim_gui/src/main/native/cpp/AddressableLEDGui.cpp
@@ -0,0 +1,160 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "AddressableLEDGui.h"
+
+#include <cstdio>
+#include <cstring>
+#include <vector>
+
+#include <hal/Ports.h>
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <mockdata/AddressableLEDData.h>
+#include <wpi/SmallVector.h>
+#include <wpi/StringRef.h>
+
+#include "ExtraGuiWidgets.h"
+#include "HALSimGui.h"
+
+using namespace halsimgui;
+
+namespace {
+struct LEDDisplaySettings {
+ int numColumns = 10;
+ LEDConfig config;
+};
+} // namespace
+
+static std::vector<LEDDisplaySettings> displaySettings;
+
+// read/write columns setting to ini file
+static void* AddressableLEDReadOpen(ImGuiContext* ctx,
+ ImGuiSettingsHandler* handler,
+ const char* name) {
+ int num;
+ if (wpi::StringRef{name}.getAsInteger(10, num)) return nullptr;
+ if (num < 0) return nullptr;
+ if (num >= static_cast<int>(displaySettings.size()))
+ displaySettings.resize(num + 1);
+ return &displaySettings[num];
+}
+
+static void AddressableLEDReadLine(ImGuiContext* ctx,
+ ImGuiSettingsHandler* handler, void* entry,
+ const char* lineStr) {
+ auto* settings = static_cast<LEDDisplaySettings*>(entry);
+ // format: columns=#
+ wpi::StringRef line{lineStr};
+ auto [name, value] = line.split('=');
+ name = name.trim();
+ value = value.trim();
+ if (name == "columns") {
+ int num;
+ if (value.getAsInteger(10, num)) return;
+ settings->numColumns = num;
+ } else if (name == "serpentine") {
+ int num;
+ if (value.getAsInteger(10, num)) return;
+ settings->config.serpentine = num != 0;
+ } else if (name == "order") {
+ int num;
+ if (value.getAsInteger(10, num)) return;
+ settings->config.order = static_cast<LEDConfig::Order>(num);
+ } else if (name == "start") {
+ int num;
+ if (value.getAsInteger(10, num)) return;
+ settings->config.start = static_cast<LEDConfig::Start>(num);
+ }
+}
+
+static void AddressableLEDWriteAll(ImGuiContext* ctx,
+ ImGuiSettingsHandler* handler,
+ ImGuiTextBuffer* out_buf) {
+ for (size_t i = 0; i < displaySettings.size(); ++i) {
+ out_buf->appendf(
+ "[AddressableLED][%d]\ncolumns=%d\nserpentine=%d\norder=%d\n"
+ "start=%d\n\n",
+ static_cast<int>(i), displaySettings[i].numColumns,
+ displaySettings[i].config.serpentine ? 1 : 0,
+ static_cast<int>(displaySettings[i].config.order),
+ static_cast<int>(displaySettings[i].config.start));
+ }
+}
+
+static void DisplayAddressableLEDs() {
+ bool hasAny = false;
+ static const int numLED = HAL_GetNumAddressableLEDs();
+ if (numLED > static_cast<int>(displaySettings.size()))
+ displaySettings.resize(numLED);
+
+ for (int i = 0; i < numLED; ++i) {
+ if (!HALSIM_GetAddressableLEDInitialized(i)) continue;
+ hasAny = true;
+
+ if (numLED > 1) ImGui::Text("LEDs[%d]", i);
+
+ static HAL_AddressableLEDData data[HAL_kAddressableLEDMaxLength];
+ int length = HALSIM_GetAddressableLEDData(i, data);
+ bool running = HALSIM_GetAddressableLEDRunning(i);
+
+ ImGui::PushItemWidth(ImGui::GetFontSize() * 6);
+ ImGui::LabelText("Length", "%d", length);
+ ImGui::LabelText("Running", "%s", running ? "Yes" : "No");
+ ImGui::InputInt("Columns", &displaySettings[i].numColumns);
+ {
+ static const char* options[] = {"Row Major", "Column Major"};
+ int val = displaySettings[i].config.order;
+ if (ImGui::Combo("Order", &val, options, 2))
+ displaySettings[i].config.order = static_cast<LEDConfig::Order>(val);
+ }
+ {
+ static const char* options[] = {"Upper Left", "Lower Left", "Upper Right",
+ "Lower Right"};
+ int val = displaySettings[i].config.start;
+ if (ImGui::Combo("Start", &val, options, 4))
+ displaySettings[i].config.start = static_cast<LEDConfig::Start>(val);
+ }
+ ImGui::Checkbox("Serpentine", &displaySettings[i].config.serpentine);
+ if (displaySettings[i].numColumns < 1) displaySettings[i].numColumns = 1;
+ ImGui::PopItemWidth();
+
+ // show as LED indicators
+ static int values[HAL_kAddressableLEDMaxLength];
+ static ImU32 colors[HAL_kAddressableLEDMaxLength];
+
+ if (!running) {
+ colors[0] = IM_COL32(128, 128, 128, 255);
+ for (int j = 0; j < length; ++j) values[j] = -1;
+ } else {
+ for (int j = 0; j < length; ++j) {
+ values[j] = j + 1;
+ colors[j] = IM_COL32(data[j].r, data[j].g, data[j].b, 255);
+ }
+ }
+
+ DrawLEDs(values, length, displaySettings[i].numColumns, colors, 0, 0,
+ displaySettings[i].config);
+ }
+ if (!hasAny) ImGui::Text("No addressable LEDs");
+}
+
+void AddressableLEDGui::Initialize() {
+ // hook ini handler to save columns settings
+ ImGuiSettingsHandler iniHandler;
+ iniHandler.TypeName = "AddressableLED";
+ iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
+ iniHandler.ReadOpenFn = AddressableLEDReadOpen;
+ iniHandler.ReadLineFn = AddressableLEDReadLine;
+ iniHandler.WriteAllFn = AddressableLEDWriteAll;
+ ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
+
+ HALSimGui::AddWindow("Addressable LEDs", DisplayAddressableLEDs,
+ ImGuiWindowFlags_AlwaysAutoResize);
+ HALSimGui::SetWindowVisibility("Addressable LEDs", HALSimGui::kHide);
+ HALSimGui::SetDefaultWindowPos("Addressable LEDs", 290, 100);
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp b/simulation/halsim_gui/src/main/native/cpp/AddressableLEDGui.h
similarity index 64%
copy from wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
copy to simulation/halsim_gui/src/main/native/cpp/AddressableLEDGui.h
index 8bd62ea..e376b9b 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/AddressableLEDGui.h
@@ -5,12 +5,13 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include "frc2/command/PrintCommand.h"
+#pragma once
-using namespace frc2;
+namespace halsimgui {
-PrintCommand::PrintCommand(const wpi::Twine& message)
- : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
-}
+class AddressableLEDGui {
+ public:
+ static void Initialize();
+};
-bool PrintCommand::RunsWhenDisabled() const { return true; }
+} // namespace halsimgui
diff --git a/simulation/halsim_gui/src/main/native/cpp/DIOGui.cpp b/simulation/halsim_gui/src/main/native/cpp/DIOGui.cpp
index 93a07f9..7c9eae5 100644
--- a/simulation/halsim_gui/src/main/native/cpp/DIOGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/DIOGui.cpp
@@ -15,6 +15,7 @@
#include <imgui.h>
#include <mockdata/DIOData.h>
#include <mockdata/DigitalPWMData.h>
+#include <mockdata/DutyCycleData.h>
#include <mockdata/EncoderData.h>
#include <mockdata/SimDeviceData.h>
@@ -33,11 +34,14 @@
static int numDIO = HAL_GetNumDigitalChannels();
static int numPWM = HAL_GetNumDigitalPWMOutputs();
static int numEncoder = HAL_GetNumEncoders();
+ static int numDutyCycle = HAL_GetNumDutyCycles();
static auto pwmMap = std::make_unique<int[]>(numDIO);
static auto encoderMap = std::make_unique<int[]>(numDIO);
+ static auto dutyCycleMap = std::make_unique<int[]>(numDIO);
std::memset(pwmMap.get(), 0, numDIO * sizeof(pwmMap[0]));
std::memset(encoderMap.get(), 0, numDIO * sizeof(encoderMap[0]));
+ std::memset(dutyCycleMap.get(), 0, numDIO * sizeof(dutyCycleMap[0]));
for (int i = 0; i < numPWM; ++i) {
if (HALSIM_GetDigitalPWMInitialized(i)) {
@@ -56,6 +60,13 @@
}
}
+ for (int i = 0; i < numDutyCycle; ++i) {
+ if (HALSIM_GetDutyCycleInitialized(i)) {
+ int channel = HALSIM_GetDutyCycleDigitalChannel(i);
+ if (channel >= 0 && channel < numDIO) dutyCycleMap[channel] = i + 1;
+ }
+ }
+
ImGui::PushItemWidth(ImGui::GetFontSize() * 8);
for (int i = 0; i < numDIO; ++i) {
if (HALSIM_GetDIOInitialized(i)) {
@@ -80,6 +91,16 @@
HALSIM_GetEncoderDigitalChannelB(encoderMap[i] - 1));
ImGui::PopStyleColor();
}
+ } else if (dutyCycleMap[i] > 0) {
+ std::snprintf(name, sizeof(name), "PWM[%d]", i);
+ if (auto simDevice =
+ HALSIM_GetDutyCycleSimDevice(dutyCycleMap[i] - 1)) {
+ LabelSimDevice(name, simDevice);
+ } else {
+ double val = HALSIM_GetDutyCycleOutput(dutyCycleMap[i] - 1);
+ if (ImGui::InputDouble(name, &val))
+ HALSIM_SetDutyCycleOutput(dutyCycleMap[i] - 1, val);
+ }
} else if (!HALSIM_GetDIOIsInput(i)) {
std::snprintf(name, sizeof(name), "Out[%d]", i);
if (auto simDevice = HALSIM_GetDIOSimDevice(i)) {
diff --git a/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp b/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp
index 3cca903..1e2f879 100644
--- a/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp
@@ -14,6 +14,7 @@
#include <imgui.h>
#include <imgui_internal.h>
#include <mockdata/DriverStationData.h>
+#include <mockdata/MockHooks.h>
#include <wpi/Format.h>
#include <wpi/SmallString.h>
#include <wpi/StringRef.h>
@@ -328,7 +329,7 @@
// Send new data every 20 ms (may be slower depending on GUI refresh rate)
static double lastNewDataTime = 0.0;
- if ((curTime - lastNewDataTime) > 0.02) {
+ if ((curTime - lastNewDataTime) > 0.02 && !HALSIM_IsTimingPaused()) {
lastNewDataTime = curTime;
HALSIM_NotifyDriverStationNewData();
}
@@ -367,7 +368,7 @@
ImGuiInputTextFlags_EnterReturnsTrue)) {
HALSIM_SetDriverStationMatchTime(matchTime);
startMatchTime = curTime - matchTime;
- } else if (!HALSIM_GetDriverStationEnabled()) {
+ } else if (!HALSIM_GetDriverStationEnabled() || HALSIM_IsTimingPaused()) {
startMatchTime = curTime - matchTime;
} else if (matchTimeEnabled) {
HALSIM_SetDriverStationMatchTime(curTime - startMatchTime);
diff --git a/simulation/halsim_gui/src/main/native/cpp/ExtraGuiWidgets.cpp b/simulation/halsim_gui/src/main/native/cpp/ExtraGuiWidgets.cpp
index 81a6855..3b1ba28 100644
--- a/simulation/halsim_gui/src/main/native/cpp/ExtraGuiWidgets.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/ExtraGuiWidgets.cpp
@@ -9,31 +9,88 @@
namespace halsimgui {
-void DrawLEDs(int* values, int numValues, int cols, const ImU32* colors,
- float size, float spacing) {
- if (numValues == 0) return;
+void DrawLEDs(const int* values, int numValues, int cols, const ImU32* colors,
+ float size, float spacing, const LEDConfig& config) {
+ if (numValues == 0 || cols < 1) return;
if (size == 0) size = ImGui::GetFontSize() / 2.0;
if (spacing == 0) spacing = ImGui::GetFontSize() / 3.0;
+ int rows = (numValues + cols - 1) / cols;
+ float inc = size + spacing;
+
ImDrawList* drawList = ImGui::GetWindowDrawList();
const ImVec2 p = ImGui::GetCursorScreenPos();
- float x = p.x + size / 2, y = p.y + size / 2;
- int rows = 1;
- for (int i = 0; i < numValues; ++i) {
- if (i >= (rows * cols)) {
- ++rows;
- x = p.x + size / 2;
- y += size + spacing;
- }
- if (values[i] > 0)
- drawList->AddRectFilled(ImVec2(x, y), ImVec2(x + size, y + size),
- colors[values[i] - 1]);
- else if (values[i] < 0)
- drawList->AddRect(ImVec2(x, y), ImVec2(x + size, y + size),
- colors[-values[i] - 1], 0.0f, 0, 1.0);
- x += size + spacing;
+
+ float ystart, yinc;
+ if (config.start & 1) {
+ // lower
+ ystart = p.y + size / 2 + inc * (rows - 1);
+ yinc = -inc;
+ } else {
+ // upper
+ ystart = p.y + size / 2;
+ yinc = inc;
}
- ImGui::Dummy(ImVec2((size + spacing) * cols, (size + spacing) * rows));
+
+ float xstart, xinc;
+ if (config.start & 2) {
+ // right
+ xstart = p.x + size / 2 + inc * (cols - 1);
+ xinc = -inc;
+ } else {
+ // left
+ xstart = p.x + size / 2;
+ xinc = inc;
+ }
+
+ float x = xstart, y = ystart;
+ if (config.order == LEDConfig::RowMajor) {
+ // row major
+ int row = 1;
+ for (int i = 0; i < numValues; ++i) {
+ if (i >= (row * cols)) {
+ ++row;
+ if (config.serpentine) {
+ x -= xinc;
+ xinc = -xinc;
+ } else {
+ x = xstart;
+ }
+ y += yinc;
+ }
+ if (values[i] > 0)
+ drawList->AddRectFilled(ImVec2(x, y), ImVec2(x + size, y + size),
+ colors[values[i] - 1]);
+ else if (values[i] < 0)
+ drawList->AddRect(ImVec2(x, y), ImVec2(x + size, y + size),
+ colors[-values[i] - 1], 0.0f, 0, 1.0);
+ x += xinc;
+ }
+ } else {
+ // column major
+ int col = 1;
+ for (int i = 0; i < numValues; ++i) {
+ if (i >= (col * rows)) {
+ ++col;
+ if (config.serpentine) {
+ y -= yinc;
+ yinc = -yinc;
+ } else {
+ y = ystart;
+ }
+ x += xinc;
+ }
+ if (values[i] > 0)
+ drawList->AddRectFilled(ImVec2(x, y), ImVec2(x + size, y + size),
+ colors[values[i] - 1]);
+ else if (values[i] < 0)
+ drawList->AddRect(ImVec2(x, y), ImVec2(x + size, y + size),
+ colors[-values[i] - 1], 0.0f, 0, 1.0);
+ y += yinc;
+ }
+ }
+
+ ImGui::Dummy(ImVec2(inc * cols, inc * rows));
}
} // namespace halsimgui
diff --git a/simulation/halsim_gui/src/main/native/cpp/PDPGui.cpp b/simulation/halsim_gui/src/main/native/cpp/PDPGui.cpp
index 038046e..da29ac3 100644
--- a/simulation/halsim_gui/src/main/native/cpp/PDPGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/PDPGui.cpp
@@ -8,6 +8,7 @@
#include "PDPGui.h"
#include <cstdio>
+#include <memory>
#include <hal/Ports.h>
#include <imgui.h>
@@ -21,6 +22,7 @@
bool hasAny = false;
static int numPDP = HAL_GetNumPDPModules();
static int numChannels = HAL_GetNumPDPChannels();
+ static auto channelCurrents = std::make_unique<double[]>(numChannels);
ImGui::PushItemWidth(ImGui::GetFontSize() * 13);
for (int i = 0; i < numPDP; ++i) {
if (HALSIM_GetPDPInitialized(i)) {
@@ -42,6 +44,7 @@
HALSIM_SetPDPVoltage(i, volts);
// channel currents; show as two columns laid out like PDP
+ HALSIM_GetPDPAllCurrents(i, channelCurrents.get());
ImGui::Text("Channel Current (A)");
ImGui::Columns(2, "channels", false);
for (int left = 0, right = numChannels - 1; left < right;
@@ -49,13 +52,13 @@
double val;
std::snprintf(name, sizeof(name), "[%d]", left);
- val = HALSIM_GetPDPCurrent(i, left);
+ val = channelCurrents[left];
if (ImGui::InputDouble(name, &val))
HALSIM_SetPDPCurrent(i, left, val);
ImGui::NextColumn();
std::snprintf(name, sizeof(name), "[%d]", right);
- val = HALSIM_GetPDPCurrent(i, right);
+ val = channelCurrents[right];
if (ImGui::InputDouble(name, &val))
HALSIM_SetPDPCurrent(i, right, val);
ImGui::NextColumn();
diff --git a/simulation/halsim_gui/src/main/native/cpp/PWMGui.cpp b/simulation/halsim_gui/src/main/native/cpp/PWMGui.cpp
index 8303f04..430502d 100644
--- a/simulation/halsim_gui/src/main/native/cpp/PWMGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/PWMGui.cpp
@@ -9,9 +9,11 @@
#include <cstdio>
#include <cstring>
+#include <memory>
#include <hal/Ports.h>
#include <imgui.h>
+#include <mockdata/AddressableLEDData.h>
#include <mockdata/PWMData.h>
#include "HALSimGui.h"
@@ -20,6 +22,19 @@
static void DisplayPWMs() {
bool hasOutputs = false;
+ static const int numPWM = HAL_GetNumPWMChannels();
+ static const int numLED = HAL_GetNumAddressableLEDs();
+ static auto ledMap = std::make_unique<int[]>(numPWM);
+
+ std::memset(ledMap.get(), 0, numPWM * sizeof(ledMap[0]));
+
+ for (int i = 0; i < numLED; ++i) {
+ if (HALSIM_GetAddressableLEDInitialized(i)) {
+ int channel = HALSIM_GetAddressableLEDOutputPort(i);
+ if (channel >= 0 && channel < numPWM) ledMap[channel] = i + 1;
+ }
+ }
+
// struct History {
// History() { std::memset(data, 0, 90 * sizeof(float)); }
// History(const History&) = delete;
@@ -30,7 +45,6 @@
//};
// static std::vector<std::unique_ptr<History>> history;
bool first = true;
- static const int numPWM = HAL_GetNumPWMChannels();
for (int i = 0; i < numPWM; ++i) {
if (HALSIM_GetPWMInitialized(i)) {
hasOutputs = true;
@@ -42,8 +56,12 @@
char name[32];
std::snprintf(name, sizeof(name), "PWM[%d]", i);
- float val = HALSimGui::AreOutputsDisabled() ? 0 : HALSIM_GetPWMSpeed(i);
- ImGui::Value(name, val, "%0.3f");
+ if (ledMap[i] > 0) {
+ ImGui::Text("%s: LED[%d]", name, ledMap[i] - 1);
+ } else {
+ float val = HALSimGui::AreOutputsDisabled() ? 0 : HALSIM_GetPWMSpeed(i);
+ ImGui::Value(name, val, "%0.3f");
+ }
// lazily build history storage
// if (static_cast<unsigned int>(i) > history.size())
diff --git a/simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp b/simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp
new file mode 100644
index 0000000..9897a7a
--- /dev/null
+++ b/simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "TimingGui.h"
+
+#include <cstdio>
+#include <cstring>
+#include <vector>
+
+#include <hal/HALBase.h>
+#include <imgui.h>
+#include <mockdata/MockHooks.h>
+#include <mockdata/NotifierData.h>
+
+#include "HALSimGui.h"
+
+using namespace halsimgui;
+
+static void DisplayTiming() {
+ int32_t status = 0;
+ uint64_t curTime = HAL_GetFPGATime(&status);
+
+ if (ImGui::Button("Run")) HALSIM_ResumeTiming();
+ ImGui::SameLine();
+ if (ImGui::Button("Pause")) HALSIM_PauseTiming();
+ ImGui::SameLine();
+ ImGui::PushButtonRepeat(true);
+ if (ImGui::Button("Step")) {
+ HALSIM_PauseTiming();
+ uint64_t nextTimeout = HALSIM_GetNextNotifierTimeout();
+ if (nextTimeout != UINT64_MAX) HALSIM_StepTiming(nextTimeout - curTime);
+ }
+ ImGui::PopButtonRepeat();
+ ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
+ ImGui::LabelText("FPGA Time", "%.3f", curTime / 1000000.0);
+ ImGui::PopItemWidth();
+
+ static std::vector<HALSIM_NotifierInfo> notifiers;
+ int32_t num = HALSIM_GetNotifierInfo(notifiers.data(), notifiers.size());
+ if (static_cast<uint32_t>(num) > notifiers.size()) {
+ notifiers.resize(num);
+ HALSIM_GetNotifierInfo(notifiers.data(), notifiers.size());
+ }
+ if (num > 0) ImGui::Separator();
+ ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
+ for (int32_t i = 0; i < num; ++i)
+ ImGui::LabelText(notifiers[i].name, "%.3f",
+ notifiers[i].timeout / 1000000.0);
+ ImGui::PopItemWidth();
+}
+
+void TimingGui::Initialize() {
+ HALSimGui::AddWindow("Timing", DisplayTiming,
+ ImGuiWindowFlags_AlwaysAutoResize);
+ HALSimGui::SetDefaultWindowPos("Timing", 5, 150);
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp b/simulation/halsim_gui/src/main/native/cpp/TimingGui.h
similarity index 64%
copy from wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
copy to simulation/halsim_gui/src/main/native/cpp/TimingGui.h
index 8bd62ea..49f33cc 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/TimingGui.h
@@ -5,12 +5,13 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include "frc2/command/PrintCommand.h"
+#pragma once
-using namespace frc2;
+namespace halsimgui {
-PrintCommand::PrintCommand(const wpi::Twine& message)
- : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
-}
+class TimingGui {
+ public:
+ static void Initialize();
+};
-bool PrintCommand::RunsWhenDisabled() const { return true; }
+} // namespace halsimgui
diff --git a/simulation/halsim_gui/src/main/native/cpp/main.cpp b/simulation/halsim_gui/src/main/native/cpp/main.cpp
index c049d1c..d5e0b16 100644
--- a/simulation/halsim_gui/src/main/native/cpp/main.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/main.cpp
@@ -9,6 +9,7 @@
#include <wpi/raw_ostream.h>
#include "AccelerometerGui.h"
+#include "AddressableLEDGui.h"
#include "AnalogGyroGui.h"
#include "AnalogInputGui.h"
#include "AnalogOutGui.h"
@@ -23,6 +24,7 @@
#include "RoboRioGui.h"
#include "SimDeviceGui.h"
#include "SolenoidGui.h"
+#include "TimingGui.h"
using namespace halsimgui;
@@ -32,6 +34,7 @@
#endif
int HALSIM_InitExtension(void) {
HALSimGui::Add(AccelerometerGui::Initialize);
+ HALSimGui::Add(AddressableLEDGui::Initialize);
HALSimGui::Add(AnalogGyroGui::Initialize);
HALSimGui::Add(AnalogInputGui::Initialize);
HALSimGui::Add(AnalogOutGui::Initialize);
@@ -45,6 +48,7 @@
HALSimGui::Add(RoboRioGui::Initialize);
HALSimGui::Add(SimDeviceGui::Initialize);
HALSimGui::Add(SolenoidGui::Initialize);
+ HALSimGui::Add(TimingGui::Initialize);
wpi::outs() << "Simulator GUI Initializing.\n";
if (!HALSimGui::Initialize()) return 0;
diff --git a/simulation/halsim_gui/src/main/native/include/ExtraGuiWidgets.h b/simulation/halsim_gui/src/main/native/include/ExtraGuiWidgets.h
index 11d83ba..7374cac 100644
--- a/simulation/halsim_gui/src/main/native/include/ExtraGuiWidgets.h
+++ b/simulation/halsim_gui/src/main/native/include/ExtraGuiWidgets.h
@@ -12,6 +12,33 @@
namespace halsimgui {
/**
+ * DrawLEDs() configuration for 2D arrays.
+ */
+struct LEDConfig {
+ /**
+ * Whether the major order is serpentined (e.g. the first row goes left to
+ * right, the second row right to left, the third row left to right, and so
+ * on).
+ */
+ bool serpentine = false;
+
+ /**
+ * The input array order (row-major or column-major).
+ */
+ enum Order { RowMajor = 0, ColumnMajor } order = RowMajor;
+
+ /**
+ * The starting location of the array (0 location).
+ */
+ enum Start {
+ UpperLeft = 0,
+ LowerLeft,
+ UpperRight,
+ LowerRight
+ } start = UpperLeft;
+};
+
+/**
* Draw a 2D array of LEDs.
*
* Values are indices into colors array. Positive values are filled (lit),
@@ -27,8 +54,10 @@
* if 0, defaults to 1/2 of font size
* @param spacing spacing between each LED (both horizontal and vertical);
* if 0, defaults to 1/3 of font size
+ * @param config 2D array configuration
*/
-void DrawLEDs(int* values, int numValues, int cols, const ImU32* colors,
- float size = 0.0f, float spacing = 0.0f);
+void DrawLEDs(const int* values, int numValues, int cols, const ImU32* colors,
+ float size = 0.0f, float spacing = 0.0f,
+ const LEDConfig& config = LEDConfig{});
} // namespace halsimgui
diff --git a/simulation/halsim_gui/src/test/native/cpp/main.cpp b/simulation/halsim_gui/src/test/native/cpp/main.cpp
index ba0d9b0..c6b6c58 100644
--- a/simulation/halsim_gui/src/test/native/cpp/main.cpp
+++ b/simulation/halsim_gui/src/test/native/cpp/main.cpp
@@ -1,17 +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. */
-/*----------------------------------------------------------------------------*/
-
-#include <hal/HAL.h>
-
-#include "gtest/gtest.h"
-
-int main(int argc, char** argv) {
- HAL_Initialize(500, 0);
- ::testing::InitGoogleTest(&argc, argv);
- int ret = RUN_ALL_TESTS();
- return ret;
-}
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include <hal/HALBase.h>
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+ HAL_Initialize(500, 0);
+ ::testing::InitGoogleTest(&argc, argv);
+ int ret = RUN_ALL_TESTS();
+ return ret;
+}
diff --git a/simulation/halsim_print/src/dev/native/cpp/main.cpp b/simulation/halsim_print/src/dev/native/cpp/main.cpp
index d9f92e9..eb9eef1 100644
--- a/simulation/halsim_print/src/dev/native/cpp/main.cpp
+++ b/simulation/halsim_print/src/dev/native/cpp/main.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -8,7 +8,7 @@
#include <chrono>
#include <thread>
-#include <hal/HAL.h>
+#include <hal/HALBase.h>
int main() {
HAL_Initialize(500, 0);
diff --git a/simulation/lowfi_simulation/build.gradle b/simulation/lowfi_simulation/build.gradle
index d86e4d6..e6e8143 100644
--- a/simulation/lowfi_simulation/build.gradle
+++ b/simulation/lowfi_simulation/build.gradle
@@ -30,16 +30,6 @@
project(':').libraryBuild.dependsOn build
- ext {
- chipObjectComponents = ["$nativeName".toString(), "${nativeName}Dev".toString(), "${nativeName}Base".toString(),
- "${nativeName}Test".toString()]
- netCommComponents = ["$nativeName".toString(), "${nativeName}Dev".toString(), "${nativeName}Base".toString(),
- "${nativeName}Test".toString()]
- useNiJava = false
- }
-
- apply from: "${rootDir}/shared/nilibraries.gradle"
-
nativeUtils.exportsConfigs {
lowfi_sim {
x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
@@ -125,6 +115,9 @@
project(':hal').addHalDependency(it, 'shared')
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
lib library: nativeName, linkage: 'shared'
+ if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
}
}
}
@@ -137,6 +130,9 @@
lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
lib library: nativeName, linkage: 'shared'
+ if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
}
}
}
diff --git a/simulation/lowfi_simulation/publish.gradle b/simulation/lowfi_simulation/publish.gradle
index 9a3204e..1fa3aaf 100644
--- a/simulation/lowfi_simulation/publish.gradle
+++ b/simulation/lowfi_simulation/publish.gradle
@@ -7,8 +7,8 @@
def outputsFolder = file("$project.buildDir/outputs")
task cppSourcesZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = zipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = zipBaseName
classifier = "sources"
from(licenseFile) {
@@ -21,8 +21,8 @@
}
task cppHeadersZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = zipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = zipBaseName
classifier = "headers"
from(licenseFile) {
diff --git a/simulation/lowfi_simulation/src/dev/native/cpp/main.cpp b/simulation/lowfi_simulation/src/dev/native/cpp/main.cpp
index 4dc5f43..eb83810 100644
--- a/simulation/lowfi_simulation/src/dev/native/cpp/main.cpp
+++ b/simulation/lowfi_simulation/src/dev/native/cpp/main.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -7,7 +7,7 @@
#include <iostream>
-#include <hal/HAL.h>
+#include <hal/HALBase.h>
int main() {
std::cout << "Hello World" << std::endl;
diff --git a/simulation/lowfi_simulation/src/test/native/cpp/lowfisim/MotorEncoderSimulatorTest.cpp b/simulation/lowfi_simulation/src/test/native/cpp/lowfisim/MotorEncoderSimulatorTest.cpp
index 83e9ddc..9166de3 100644
--- a/simulation/lowfi_simulation/src/test/native/cpp/lowfisim/MotorEncoderSimulatorTest.cpp
+++ b/simulation/lowfi_simulation/src/test/native/cpp/lowfisim/MotorEncoderSimulatorTest.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -49,7 +49,7 @@
frc::sim::lowfi::MotorEncoderConnector connector(motorSim, encoderSim);
talon.Set(0.5);
- motorSim.Update(.02);
+ motorSim.Update(0.02);
connector.Update();
// Position
@@ -64,7 +64,7 @@
TEST(MotorEncoderConnectorTest, TestWithDistancePerPulseFullSpeed) {
frc::Talon talon{3};
frc::Encoder encoder{3, 1};
- encoder.SetDistancePerPulse(.001);
+ encoder.SetDistancePerPulse(0.001);
frc::sim::lowfi::SimpleMotorModel motorModelSim(6000);
frc::sim::lowfi::WpiMotorSim motorSim(3, motorModelSim);
@@ -88,7 +88,7 @@
TEST(MotorEncoderConnectorTest, TestWithDistancePerPulseRealistic) {
frc::Talon talon{3};
frc::Encoder encoder{3, 1};
- encoder.SetDistancePerPulse(.001);
+ encoder.SetDistancePerPulse(0.001);
frc::sim::lowfi::SimpleMotorModel motorModelSim(6000);
frc::sim::lowfi::WpiMotorSim motorSim(3, motorModelSim);
@@ -97,7 +97,7 @@
talon.Set(0.5);
- motorSim.Update(.02);
+ motorSim.Update(0.02);
connector.Update();
// Position
diff --git a/simulation/lowfi_simulation/src/test/native/cpp/lowfisim/motormodel/SimpleMotorModelSimulationTest.cpp b/simulation/lowfi_simulation/src/test/native/cpp/lowfisim/motormodel/SimpleMotorModelSimulationTest.cpp
index e60cf1e..21c5e7e 100644
--- a/simulation/lowfi_simulation/src/test/native/cpp/lowfisim/motormodel/SimpleMotorModelSimulationTest.cpp
+++ b/simulation/lowfi_simulation/src/test/native/cpp/lowfisim/motormodel/SimpleMotorModelSimulationTest.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -13,7 +13,7 @@
// Test forward voltage
motorModelSim.SetVoltage(6);
- motorModelSim.Update(.5);
+ motorModelSim.Update(0.5);
EXPECT_DOUBLE_EQ(50, motorModelSim.GetPosition());
EXPECT_DOUBLE_EQ(100, motorModelSim.GetVelocity());
@@ -26,7 +26,7 @@
// Test negative voltage
motorModelSim.Reset();
motorModelSim.SetVoltage(-3);
- motorModelSim.Update(.06);
+ motorModelSim.Update(0.06);
EXPECT_DOUBLE_EQ(-3, motorModelSim.GetPosition());
EXPECT_DOUBLE_EQ(-50, motorModelSim.GetVelocity());
diff --git a/simulation/lowfi_simulation/src/test/native/cpp/main.cpp b/simulation/lowfi_simulation/src/test/native/cpp/main.cpp
index 0e00efa..c6b6c58 100644
--- a/simulation/lowfi_simulation/src/test/native/cpp/main.cpp
+++ b/simulation/lowfi_simulation/src/test/native/cpp/main.cpp
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* 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. */
/*----------------------------------------------------------------------------*/
-#include <hal/HAL.h>
+#include <hal/HALBase.h>
#include "gtest/gtest.h"
diff --git a/styleguide/checkstyle.xml b/styleguide/checkstyle.xml
index 7bc388e..758cee4 100644
--- a/styleguide/checkstyle.xml
+++ b/styleguide/checkstyle.xml
@@ -51,6 +51,26 @@
<property name="message"
value="Avoid using corresponding octal or Unicode escape." />
</module>
+ <module name="IllegalTokenText">
+ <property name="tokens"
+ value="NUM_INT,NUM_LONG"/>
+ <property name="format"
+ value="^0[^lx]"/>
+ <property name="ignoreCase"
+ value="true"/>
+ <property name="message"
+ value="Forbid leading zeros in an integer literal, other than zero and a hex literal." />
+ </module>
+ <module name="IllegalTokenText">
+ <property name="tokens"
+ value="NUM_DOUBLE,NUM_FLOAT"/>
+ <property name="format"
+ value="(^\.|\.$)"/>
+ <property name="ignoreCase"
+ value="true"/>
+ <property name="message"
+ value="Must use leading and trailing zero in floating point numbers." />
+ </module>
<module name="AvoidEscapedUnicodeCharacters">
<property name="allowEscapesForControlCharacters"
value="true" />
diff --git a/test-scripts/README.md b/test-scripts/README.md
index a96764e..d09cb8e 100644
--- a/test-scripts/README.md
+++ b/test-scripts/README.md
@@ -1,17 +1,19 @@
-# WPILIB TEST SCRIPTS
+# WPILib Test Scripts
+
## Overview
+
These test scripts are designed to allow the user of the WPILib test framework to quickly and easily deploy and run their tests on the WPI roboRIO.
-In order for the automated test system to work there is a driverstation onboard the roboRIO that handles a queue of users waiting to use the driver station. All of the interaction with this queue is handled internally by test scripts contained within this folder.
-
-If you deploy code to the test stand using the Eclipse plugins, you _must_ remove the build artifacts in `/home/lvuser`, or you will break tests.
+If you deploy code to the test stand using GradleRIO, you _must_ remove the build artifacts in `/home/lvuser`, or you will break the test stand.
## roboRIO Setup
-The roboRIO on the test bench must be updated everytime NI releases a new image.
+The roboRIO on the test bench must be updated every time NI releases a new image.
-1. [Use the roboRIO Imaging Tool to format the roboRIO with the lastest image.](https://wpilib.screenstepslive.com/s/4485/m/13503/l/144984-imaging-your-roborio)
-2. [Install Java on the roboRIO.](https://wpilib.screenstepslive.com/s/4485/m/13503/l/599747-installing-java-8-on-the-roborio-using-the-frc-roborio-java-installer-java-only)
-3. SFTP the [teststand, netconsole, and libstdc++ ipk files](https://users.wpi.edu/~phplenefisch/ipk/) on to the roboRIO.
-4. ssh on to the roboRIO as the admin user (ex: `ssh admin@roboRIO-190-FRC.local`)
-5. Use opkg to install the ipk files (ex: `opkg install teststand_1.2-1_armv7a-vfp.ipk`)
-6. Reboot the roboRIO
+1. [Use the roboRIO Imaging Tool to format the roboRIO with the lastest image.](https://frcdocs.wpi.edu/en/latest/docs/getting-started/getting-started-frc-control-system/imaging-your-roborio.html)
+2. Set a static ip on the roboRIO web dashboard to `10.1.90.2`
+2. Install Java on the roboRIO
+ 1. [Download the JRE from Maven.](https://frcmaven.wpi.edu/artifactory/list/release/edu/wpi/first/jdk/)
+ 2. Transfer the JRE ipk to the roboRIO with scp: `scp <local path> admin@roboRIO-190-FRC.local:/tmp/frcjre.ipk`
+ 3. Install the JRE: `opkg install /tmp/frcjre.ipk`
+ 4. Remove the ipk file: `rm /tmp/frcjre.ipk`
+3. Reboot the roboRIO
diff --git a/test-scripts/config.sh b/test-scripts/config.sh
index 2232b8d..be2587a 100755
--- a/test-scripts/config.sh
+++ b/test-scripts/config.sh
@@ -1,9 +1,9 @@
#!/usr/bin/env bash
#*----------------------------------------------------------------------------*#
-#* Copyright (c) FIRST 2014. All Rights Reserved. *#
+#* 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. *#
+#* the project. *#
#*----------------------------------------------------------------------------*#
# If this is changed, update the .gitignore
@@ -31,7 +31,7 @@
DEFAULT_JAVA_TEST_NAME=FRCUserProgram.jar
DEFAULT_JAVA_TEST_ARGS=""
-DEFAULT_LOCAL_JAVA_TEST_FILE=../build/integrationTestFiles/java/wpilibjIntegrationTests.jar
+DEFAULT_LOCAL_JAVA_TEST_FILE=../build/integrationTestFiles/java/wpilibjIntegrationTests-all.jar
JAVA_REPORT=javareport.xml
DEFAULT_LIBRARY_NATIVE_FILES=../build/integrationTestFiles/libs
diff --git a/test-scripts/deploy-and-run-test-on-robot.sh b/test-scripts/deploy-and-run-test-on-robot.sh
index f023a18..ca5f59f 100755
--- a/test-scripts/deploy-and-run-test-on-robot.sh
+++ b/test-scripts/deploy-and-run-test-on-robot.sh
@@ -1,9 +1,9 @@
#!/usr/bin/env bash
#*----------------------------------------------------------------------------*#
-#* Copyright (c) FIRST 2014. All Rights Reserved. *#
+#* 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. *#
+#* the project. *#
#*----------------------------------------------------------------------------*#
# Configurable variables
@@ -19,7 +19,7 @@
usage="$(basename "$0") [-h] (java|cpp) [-A] [arg] [arg]...
A script designed to run the integration tests.
-This script should only be run on the roborio.
+This script should only be run on the computer connected to the roboRIO.
Where:
-h Show this help text.
-A Disable language recomended arguments.
@@ -30,20 +30,26 @@
LANGUAGE=none
LOCAL_TEST_FILE=none
DESTINATION_TEST_FILE=none
-LIBRARY_FILES=none
TEST_RUN_ARGS=""
+DESTINATION_TEST_RESULTS=none
+LOCAL_TEST_RESULTS=none
-# Begin searching for options from the third paramater on
+
+# Begin searching for options from the second paramater on
PARAM_ARGS=${@:2}
if [[ "$1" = java ]]; then
LANGUAGE=$1
LOCAL_TEST_FILE=$DEFAULT_LOCAL_JAVA_TEST_FILE
DESTINATION_TEST_FILE=$DEFAULT_DESTINATION_JAVA_TEST_FILE
+ DESTINATION_TEST_RESULTS=$DEFAULT_DESTINATION_JAVA_TEST_RESULTS
+ LOCAL_TEST_RESULTS=$DEFAULT_LOCAL_JAVA_TEST_RESULT
elif [[ "$1" = cpp ]]; then
LANGUAGE=$1
LOCAL_TEST_FILE=$DEFAULT_LOCAL_CPP_TEST_FILE
DESTINATION_TEST_FILE=$DEFAULT_DESTINATION_CPP_TEST_FILE
+ DESTINATION_TEST_RESULTS=$DEFAULT_DESTINATION_CPP_TEST_RESULTS
+ LOCAL_TEST_RESULTS=$DEFAULT_LOCAL_CPP_TEST_RESULT
elif [[ "$1" = "-h" ]]; then
printf "Usage:\n"
echo "$usage"
@@ -65,25 +71,14 @@
shopt -s huponexit
-SCP_TEST_SCRIPT="scp config.sh ${DEFAULT_LOCAL_RUN_TEST_SCRIPT} ${ROBOT_ADDRESS}:/${DEFAULT_DESTINATION_DIR}"
-SSH_CHMOD_AND_MAKE_TEMP_TEST_DIR="ssh -t ${ROBOT_ADDRESS} \"chmod a+x ${DEFAULT_DESTINATION_RUN_TEST_SCRIPT}; mkdir ${DEFAULT_TEST_SCP_DIR}; touch ${DESTINATION_TEST_FILE}\""
-SCP_TEST_PROGRAM="scp ${LOCAL_TEST_FILE} ${ROBOT_ADDRESS}:${DESTINATION_TEST_FILE}"
-SSH_RUN_TESTS="ssh -t ${ROBOT_ADDRESS} ${DEFAULT_DESTINATION_RUN_TEST_SCRIPT} ${LANGUAGE} $(whoami) -d ${DEFAULT_TEST_SCP_DIR} ${TEST_RUN_ARGS}"
-SCP_NATIVE_LIBRARIES="scp ${DEFAULT_LIBRARY_NATIVE_FILES}/* ${ROBOT_ADDRESS}:${DEFAULT_LIBRARY_NATIVE_DESTINATION}"
-CONFIG_NATIVE_LIBRARIES="ssh -t ${ADMIN_ROBOT_ADDRESS} ldconfig"
+# Fail if any command fails
+set -e
-if [ $(which sshpass) ]; then
- sshpass -p "" ${SCP_NATIVE_LIBRARIES}
- sshpass -p "" ${CONFIG_NATIVE_LIBRARIES}
- sshpass -p "" ${SCP_TEST_SCRIPT}
- sshpass -p "" ${SSH_CHMOD_AND_MAKE_TEMP_TEST_DIR}
- sshpass -p "" ${SCP_TEST_PROGRAM}
- sshpass -p "" ${SSH_RUN_TESTS}
-else
- eval ${SCP_NATIVE_LIBRARIES}
- eval ${CONFIG_NATIVE_LIBRARIES}
- eval ${SCP_TEST_SCRIPT}
- eval ${SSH_CHMOD_AND_MAKE_TEMP_TEST_DIR}
- eval ${SCP_TEST_PROGRAM}
- eval ${SSH_RUN_TESTS}
-fi
+ssh ${ROBOT_ADDRESS} "rm -R ${DEFAULT_DESTINATION_TEST_RESULTS_DIR}; mkdir ${DEFAULT_DESTINATION_TEST_RESULTS_DIR}"
+scp ${DEFAULT_LIBRARY_NATIVE_FILES}/* ${ROBOT_ADDRESS}:${DEFAULT_LIBRARY_NATIVE_DESTINATION}
+ssh ${ADMIN_ROBOT_ADDRESS} ldconfig
+scp config.sh ${DEFAULT_LOCAL_RUN_TEST_SCRIPT} ${ROBOT_ADDRESS}:/${DEFAULT_DESTINATION_DIR}
+ssh ${ROBOT_ADDRESS} "chmod a+x ${DEFAULT_DESTINATION_RUN_TEST_SCRIPT}; mkdir ${DEFAULT_TEST_SCP_DIR}; touch ${DESTINATION_TEST_FILE}"
+scp ${LOCAL_TEST_FILE} ${ROBOT_ADDRESS}:${DESTINATION_TEST_FILE}
+ssh ${ROBOT_ADDRESS} ${DEFAULT_DESTINATION_RUN_TEST_SCRIPT} ${LANGUAGE} -d ${DEFAULT_TEST_SCP_DIR} ${TEST_RUN_ARGS}
+mkdir ${DEFAULT_LOCAL_TEST_RESULTS_DIR}; scp ${ROBOT_ADDRESS}:${DESTINATION_TEST_RESULTS} ${LOCAL_TEST_RESULTS}
diff --git a/test-scripts/jenkins-run-tests-get-results.sh b/test-scripts/jenkins-run-tests-get-results.sh
deleted file mode 100755
index 8981b4d..0000000
--- a/test-scripts/jenkins-run-tests-get-results.sh
+++ /dev/null
@@ -1,79 +0,0 @@
-#!/usr/bin/env bash
-#*----------------------------------------------------------------------------*#
-#* Copyright (c) FIRST 2014. All Rights Reserved. *#
-#* Open Source Software - may be modified and shared by FRC teams. The code *#
-#* must be accompanied by the FIRST BSD license file in the root directory of *#
-#* the project. *#
-#*----------------------------------------------------------------------------*#
-
-# Configurable variables
-source config.sh
-
-(
-# Wait for lock
-printf "Getting exclusive lock for RIO execution...\n"
-flock -x 200 || exit 1
-
-# Ensure the teststand is dead
-SSH_STOP_TESTSTAND="ssh -t ${ROBOT_ADDRESS} sh -c '/etc/init.d/teststand stop; sleep 1'"
-if [ $(which sshpass) ]; then
- sshpass -p "" ${SSH_STOP_TESTSTAND}
-else
- eval ${SSH_STOP_TESTSTAND}
-fi
-
-# If there are already test results in the repository then remove them
-if [[ -e ${DEFAULT_LOCAL_TEST_RESULTS_DIR} ]]; then
- rm -R ${DEFAULT_LOCAL_TEST_RESULTS_DIR}
-fi
-
-# Make the directory where the tests should live
-mkdir ${DEFAULT_LOCAL_TEST_RESULTS_DIR} 2>/dev/null
-
-# Remove the preivous test results from the the robot
-SSH_REMOVE_OLD_TEST_RESULTS="ssh -t ${ROBOT_ADDRESS} rm -R ${DEFAULT_DESTINATION_TEST_RESULTS_DIR}; mkdir ${DEFAULT_DESTINATION_TEST_RESULTS_DIR}"
-if [ $(which sshpass) ]; then
- sshpass -p "" ${SSH_REMOVE_OLD_TEST_RESULTS}
-else
- eval ${SSH_REMOVE_OLD_TEST_RESULTS}
-fi
-
-printf "Running cpp test\n"
-
-# Run the C++ Tests
-./deploy-and-run-test-on-robot.sh cpp -A "--gtest_output=xml:${DEFAULT_DESTINATION_CPP_TEST_RESULTS}"
-
-# Retrive the C++ Test Results
-SCP_GET_CPP_TEST_RESULT="scp ${ROBOT_ADDRESS}:${DEFAULT_DESTINATION_CPP_TEST_RESULTS} ${DEFAULT_LOCAL_CPP_TEST_RESULT}"
-if [ $(which sshpass) ]; then
- sshpass -p "" ${SCP_GET_CPP_TEST_RESULT}
-else
- eval ${SCP_GET_CPP_TEST_RESULT}
-fi
-
-sleep 10
-
-# Run the Java Tests
-./deploy-and-run-test-on-robot.sh java
-
-# Retrive the Java Test Results
-SCP_GET_JAVA_TEST_RESULT="scp ${ROBOT_ADDRESS}:${DEFAULT_DESTINATION_JAVA_TEST_RESULTS} ${DEFAULT_LOCAL_JAVA_TEST_RESULT}"
-if [ $(which sshpass) ]; then
- sshpass -p "" ${SCP_GET_JAVA_TEST_RESULT}
-else
- eval ${SCP_GET_JAVA_TEST_RESULT}
-fi
-
-# Make sure that we got test results back.
-if [ ! -e ${DEFAULT_LOCAL_CPP_TEST_RESULT} ]; then
- echo "There are no results from the C++ tests; they must have failed."
- exit 100
-fi
-
-if [ ! -e ${DEFAULT_LOCAL_JAVA_TEST_RESULT} ]; then
- echo "There are no results from the Java tests; they must have failed."
- exit 101
-fi
-
-# The mutex is released when this program exits
-) 200>/var/lock/jenkins.rio.exclusivelock
diff --git a/test-scripts/run-tests-on-robot.sh b/test-scripts/run-tests-on-robot.sh
index 67d54f2..96d10c0 100755
--- a/test-scripts/run-tests-on-robot.sh
+++ b/test-scripts/run-tests-on-robot.sh
@@ -1,9 +1,9 @@
#!/usr/bin/env bash
#*----------------------------------------------------------------------------*#
-#* Copyright (c) FIRST 2014. All Rights Reserved. *#
+#* 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. *#
+#* the project. *#
#*----------------------------------------------------------------------------*#
# This file is intended to be run in the DEFAULT_TEST_DIR on the roboRIO.
@@ -17,12 +17,11 @@
DEFAULT_TEST_DIR=${DEFAULT_DESTINATION_DIR}
DEFAULT_PATH_TO_JRE=/usr/local/frc/JRE/bin/java
-usage="$(basename "$0") [-h] (java|cpp) name [-d test_dir] [-A] [arg] [arg]...
+usage="$(basename "$0") [-h] (java|cpp) [-d test_dir] [-A] [arg] [arg]...
A script designed to run the integration tests.
This script should only be run on the roborio.
Where:
-h Show this help text
- name The name of the user trying to run the tests (used for driver station)
-d The directory where the tests have been placed.
This is done to prevent overwriting an already running program.
This scrip will automatically move the test into the ${DEFAULT_TEST_DIR}
@@ -42,12 +41,11 @@
LANGUAGE=none
TEST_FILE=none
-NAME=$2
TEST_DIR="$DEFAULT_TEST_DIR"
-# Begin searching for options from the third paramater on
-PARAM_ARGS=${@:3}
+# Begin searching for options from the second paramater on
+PARAM_ARGS=${@:2}
# Where the test arguments start
-TEST_RUN_ARGS=${@:3}
+TEST_RUN_ARGS=${@:2}
RUN_WITH_DEFAULT_ARGS=true
DEFAULT_ARGS=""
@@ -70,7 +68,7 @@
exit 1
fi
-PARAM_COUNTER=2
+PARAM_COUNTER=1
printf "Param Args ${PARAM_ARGS}\n"
# Check for optional paramaters
diff --git a/wpilibNewCommands/.styleguide b/wpilibNewCommands/.styleguide
new file mode 100644
index 0000000..11a1339
--- /dev/null
+++ b/wpilibNewCommands/.styleguide
@@ -0,0 +1,29 @@
+cppHeaderFileInclude {
+ \.h$
+ \.hpp$
+ \.inc$
+}
+
+cppSrcFileInclude {
+ \.cpp$
+}
+
+repoRootNameOverride {
+ wpilib
+}
+
+includeOtherLibs {
+ ^cameraserver/
+ ^cscore
+ ^frc/
+ ^hal/
+ ^imgui
+ ^mockdata/
+ ^networktables/
+ ^ntcore
+ ^opencv2/
+ ^support/
+ ^units/
+ ^vision/
+ ^wpi/
+}
diff --git a/wpilibNewCommands/WPILibNewCommands.json b/wpilibNewCommands/WPILibNewCommands.json
new file mode 100644
index 0000000..9a22dbd
--- /dev/null
+++ b/wpilibNewCommands/WPILibNewCommands.json
@@ -0,0 +1,37 @@
+{
+ "fileName": "WPILibNewCommands.json",
+ "name": "WPILib-New-Commands",
+ "version": "2020.0.0",
+ "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+ "mavenUrls": [],
+ "jsonUrl": "",
+ "javaDependencies": [
+ {
+ "groupId": "edu.wpi.first.wpilibNewCommands",
+ "artifactId": "wpilibNewCommands-java",
+ "version": "wpilib"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "edu.wpi.first.wpilibNewCommands",
+ "artifactId": "wpilibNewCommands-cpp",
+ "version": "wpilib",
+ "libName": "wpilibNewCommands",
+ "headerClassifier": "headers",
+ "sourcesClassifier": "sources",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "linuxathena",
+ "linuxraspbian",
+ "linuxaarch64bionic",
+ "windowsx86-64",
+ "windowsx86",
+ "linuxx86-64",
+ "osxx86-64"
+ ]
+ }
+ ]
+}
diff --git a/wpilibNewCommands/build.gradle b/wpilibNewCommands/build.gradle
new file mode 100644
index 0000000..09f2a95
--- /dev/null
+++ b/wpilibNewCommands/build.gradle
@@ -0,0 +1,97 @@
+ext {
+ nativeName = 'wpilibNewCommands'
+ devMain = 'edu.wpi.first.wpilibj.commands.DevMain'
+}
+
+evaluationDependsOn(':ntcore')
+evaluationDependsOn(':cscore')
+evaluationDependsOn(':hal')
+evaluationDependsOn(':wpilibc')
+evaluationDependsOn(':cameraserver')
+evaluationDependsOn(':wpilibj')
+
+apply from: "${rootDir}/shared/javacpp/setupBuild.gradle"
+
+dependencies {
+ implementation project(':wpiutil')
+ implementation project(':ntcore')
+ implementation project(':cscore')
+ implementation project(':hal')
+ implementation project(':wpilibj')
+ devImplementation project(':wpiutil')
+ devImplementation project(':ntcore')
+ devImplementation project(':cscore')
+ devImplementation project(':hal')
+ devImplementation project(':wpilibj')
+ testImplementation 'com.google.guava:guava:19.0'
+ testImplementation 'org.mockito:mockito-core:2.27.0'
+}
+
+nativeUtils.exportsConfigs {
+ wpilibNewCommands {
+ x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
+ '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
+ '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
+ '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+ x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
+ '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
+ '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
+ '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+ }
+}
+
+apply plugin: DisableBuildingGTest
+
+model {
+ components {}
+ binaries {
+ all {
+ if (!it.buildable || !(it instanceof NativeBinarySpec)) {
+ return
+ }
+ lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+ lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
+ project(':hal').addHalDependency(it, 'shared')
+ lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+
+ if (it.component.name == "${nativeName}Dev") {
+ lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
+ project(':hal').addHalJniDependency(it)
+ }
+
+ if (it instanceof GoogleTestTestSuiteBinarySpec) {
+ nativeUtils.useRequiredLibrary(it, 'opencv_shared')
+ lib project: ':cscore', library: 'cscore', linkage: 'shared'
+ }
+ if ((it instanceof NativeExecutableBinarySpec || it instanceof GoogleTestTestSuiteBinarySpec) && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
+ }
+ }
+ tasks {
+ def c = $.components
+ def found = false
+ def systemArch = getCurrentArch()
+ c.each {
+ if (it in NativeExecutableSpec && it.name == "${nativeName}Dev") {
+ it.binaries.each {
+ if (!found) {
+ def arch = it.targetPlatform.name
+ if (arch == systemArch) {
+ def filePath = it.tasks.install.installDirectory.get().toString() + File.separatorChar + 'lib'
+
+ found = true
+ }
+ }
+ }
+ }
+ }
+ }
+}
+
+test {
+ testLogging {
+ outputs.upToDateWhen {false}
+ showStandardStreams = true
+ }
+}
diff --git a/wpilibNewCommands/src/dev/java/edu/wpi/first/wpilibj2/commands/DevMain.java b/wpilibNewCommands/src/dev/java/edu/wpi/first/wpilibj2/commands/DevMain.java
new file mode 100644
index 0000000..ca6ce63
--- /dev/null
+++ b/wpilibNewCommands/src/dev/java/edu/wpi/first/wpilibj2/commands/DevMain.java
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* 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.wpilibj2.commands;
+
+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/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java b/wpilibNewCommands/src/dev/native/cpp/main.cpp
similarity index 61%
copy from wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
copy to wpilibNewCommands/src/dev/native/cpp/main.cpp
index df39cd1..5312a1d 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
+++ b/wpilibNewCommands/src/dev/native/cpp/main.cpp
@@ -1,16 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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.command;
-
-/**
- * A class to simulate a simple subsystem.
- */
-public class MockSubsystem extends Subsystem {
- @Override
- protected void initDefaultCommand() {}
-}
+int main() {}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/Command.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
similarity index 92%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
index 0aa9345..6cca974 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
@@ -80,7 +80,7 @@
* @param seconds the timeout duration
* @return the command with the timeout added
*/
- default Command withTimeout(double seconds) {
+ default ParallelRaceGroup withTimeout(double seconds) {
return new ParallelRaceGroup(this, new WaitCommand(seconds));
}
@@ -99,7 +99,7 @@
* @param condition the interrupt condition
* @return the command with the interrupt condition added
*/
- default Command withInterrupt(BooleanSupplier condition) {
+ default ParallelRaceGroup withInterrupt(BooleanSupplier condition) {
return new ParallelRaceGroup(this, new WaitUntilCommand(condition));
}
@@ -112,11 +112,12 @@
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
* further decorated without issue.
*
- * @param toRun the Runnable to run
+ * @param toRun the Runnable to run
+ * @param requirements the required subsystems
* @return the decorated command
*/
- default Command beforeStarting(Runnable toRun) {
- return new SequentialCommandGroup(new InstantCommand(toRun), this);
+ default SequentialCommandGroup beforeStarting(Runnable toRun, Subsystem... requirements) {
+ return new SequentialCommandGroup(new InstantCommand(toRun, requirements), this);
}
/**
@@ -128,11 +129,12 @@
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
* further decorated without issue.
*
- * @param toRun the Runnable to run
+ * @param toRun the Runnable to run
+ * @param requirements the required subsystems
* @return the decorated command
*/
- default Command andThen(Runnable toRun) {
- return new SequentialCommandGroup(this, new InstantCommand(toRun));
+ default SequentialCommandGroup andThen(Runnable toRun, Subsystem... requirements) {
+ return new SequentialCommandGroup(this, new InstantCommand(toRun, requirements));
}
/**
@@ -148,7 +150,7 @@
* @param next the commands to run next
* @return the decorated command
*/
- default Command andThen(Command... next) {
+ default SequentialCommandGroup andThen(Command... next) {
SequentialCommandGroup group = new SequentialCommandGroup(this);
group.addCommands(next);
return group;
@@ -168,7 +170,7 @@
* @param parallel the commands to run in parallel
* @return the decorated command
*/
- default Command deadlineWith(Command... parallel) {
+ default ParallelDeadlineGroup deadlineWith(Command... parallel) {
return new ParallelDeadlineGroup(this, parallel);
}
@@ -186,7 +188,7 @@
* @param parallel the commands to run in parallel
* @return the decorated command
*/
- default Command alongWith(Command... parallel) {
+ default ParallelCommandGroup alongWith(Command... parallel) {
ParallelCommandGroup group = new ParallelCommandGroup(this);
group.addCommands(parallel);
return group;
@@ -206,7 +208,7 @@
* @param parallel the commands to run in parallel
* @return the decorated command
*/
- default Command raceWith(Command... parallel) {
+ default ParallelRaceGroup raceWith(Command... parallel) {
ParallelRaceGroup group = new ParallelRaceGroup(this);
group.addCommands(parallel);
return group;
@@ -224,7 +226,7 @@
*
* @return the decorated command
*/
- default Command perpetually() {
+ default PerpetualCommand perpetually() {
return new PerpetualCommand(this);
}
@@ -235,7 +237,7 @@
*
* @return the decorated command
*/
- default Command asProxy() {
+ default ProxyScheduleCommand asProxy() {
return new ProxyScheduleCommand(this);
}
@@ -279,7 +281,7 @@
* Whether the command requires a given subsystem. Named "hasRequirement" rather than "requires"
* to avoid confusion with
* {@link edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.Subsystem)}
- * - this may be able to be changed in a few years.
+ * - this may be able to be changed in a few years.
*
* @param requirement the subsystem to inquire about
* @return whether the subsystem is required
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
new file mode 100644
index 0000000..af50dc8
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
@@ -0,0 +1,353 @@
+/*----------------------------------------------------------------------------*/
+/* 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.wpilibj2.command;
+
+import java.util.function.Consumer;
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+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.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that uses two PID controllers ({@link PIDController}) and a
+ * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
+ * {@link Trajectory} with a mecanum drive.
+ *
+ * <p>The command handles trajectory-following,
+ * Velocity PID calculations, and feedforwards internally. This
+ * is intended to be a more-or-less "complete solution" that can be used by teams without a great
+ * deal of controls expertise.
+ *
+ * <p>Advanced teams seeking more flexibility (for example, those who wish to use the onboard
+ * PID functionality of a "smart" motor controller) may use the secondary constructor that omits
+ * the PID and feedforward functionality, returning only the raw wheel speeds from the PID
+ * controllers.
+ *
+ * <p>The robot angle controller does not follow the angle given by
+ * the trajectory but rather goes to the angle given in the final state of the trajectory.
+ */
+
+@SuppressWarnings({"PMD.TooManyFields", "MemberName"})
+public class MecanumControllerCommand extends CommandBase {
+ private final Timer m_timer = new Timer();
+ private MecanumDriveWheelSpeeds m_prevSpeeds;
+ private double m_prevTime;
+ private Pose2d m_finalPose;
+ private final boolean m_usePID;
+
+ private final Trajectory m_trajectory;
+ private final Supplier<Pose2d> m_pose;
+ private final SimpleMotorFeedforward m_feedforward;
+ private final MecanumDriveKinematics m_kinematics;
+ private final PIDController m_xController;
+ private final PIDController m_yController;
+ private final ProfiledPIDController m_thetaController;
+ private final double m_maxWheelVelocityMetersPerSecond;
+ private final PIDController m_frontLeftController;
+ private final PIDController m_rearLeftController;
+ private final PIDController m_frontRightController;
+ private final PIDController m_rearRightController;
+ private final Supplier<MecanumDriveWheelSpeeds> m_currentWheelSpeeds;
+ private final Consumer<MecanumDriveMotorVoltages> m_outputDriveVoltages;
+ private final Consumer<MecanumDriveWheelSpeeds> m_outputWheelSpeeds;
+
+ /**
+ * Constructs a new MecanumControllerCommand that when executed will follow the provided
+ * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to
+ * 12 as a voltage output to the motor.
+ *
+ * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
+ * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
+ *
+ * <p>Note 2: The rotation controller will calculate the rotation based on the final pose in the
+ * trajectory, not the poses at each time step.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose - use one of
+ * the odometry classes to provide this.
+ * @param feedforward The feedforward to use for the drivetrain.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param xController The Trajectory Tracker PID controller
+ * for the robot's x position.
+ * @param yController The Trajectory Tracker PID controller
+ * for the robot's y position.
+ * @param thetaController The Trajectory Tracker PID controller
+ * for angle for the robot.
+ * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
+ * @param frontLeftController The front left wheel velocity PID.
+ * @param rearLeftController The rear left wheel velocity PID.
+ * @param frontRightController The front right wheel velocity PID.
+ * @param rearRightController The rear right wheel velocity PID.
+ * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing
+ * the current wheel speeds.
+ * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing
+ * the output motor voltages.
+ * @param requirements The subsystems to require.
+ */
+
+ @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
+ public MecanumControllerCommand(Trajectory trajectory,
+ Supplier<Pose2d> pose,
+ SimpleMotorFeedforward feedforward,
+ MecanumDriveKinematics kinematics,
+
+ PIDController xController,
+ PIDController yController,
+ ProfiledPIDController thetaController,
+
+ double maxWheelVelocityMetersPerSecond,
+
+ PIDController frontLeftController,
+ PIDController rearLeftController,
+ PIDController frontRightController,
+ PIDController rearRightController,
+
+ Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
+
+ Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
+ Subsystem... requirements) {
+ m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
+ m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
+ m_feedforward = requireNonNullParam(feedforward, "feedforward", "MecanumControllerCommand");
+ m_kinematics = requireNonNullParam(kinematics, "kinematics", "MecanumControllerCommand");
+
+ m_xController = requireNonNullParam(xController, "xController",
+ "MecanumControllerCommand");
+ m_yController = requireNonNullParam(yController, "yController",
+ "MecanumControllerCommand");
+ m_thetaController = requireNonNullParam(thetaController, "thetaController",
+ "MecanumControllerCommand");
+
+ m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond,
+ "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
+
+ m_frontLeftController = requireNonNullParam(frontLeftController,
+ "frontLeftController", "MecanumControllerCommand");
+ m_rearLeftController = requireNonNullParam(rearLeftController,
+ "rearLeftController", "MecanumControllerCommand");
+ m_frontRightController = requireNonNullParam(frontRightController,
+ "frontRightController", "MecanumControllerCommand");
+ m_rearRightController = requireNonNullParam(rearRightController,
+ "rearRightController", "MecanumControllerCommand");
+
+ m_currentWheelSpeeds = requireNonNullParam(currentWheelSpeeds,
+ "currentWheelSpeeds", "MecanumControllerCommand");
+
+ m_outputDriveVoltages = requireNonNullParam(outputDriveVoltages,
+ "outputDriveVoltages", "MecanumControllerCommand");
+
+ m_outputWheelSpeeds = null;
+
+ m_usePID = true;
+
+ addRequirements(requirements);
+ }
+
+ /**
+ * Constructs a new MecanumControllerCommand that when executed will follow the provided
+ * trajectory. The user should implement a velocity PID on the desired output wheel velocities.
+ *
+ * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path -
+ * this is left to the user, since it is not appropriate for paths with non-stationary end-states.
+ *
+ * <p>Note2: The rotation controller will calculate the rotation based on the final pose
+ * in the trajectory, not the poses at each time step.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose - use one of
+ * the odometry classes to provide this.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param xController The Trajectory Tracker PID controller
+ * for the robot's x position.
+ * @param yController The Trajectory Tracker PID controller
+ * for the robot's y position.
+ * @param thetaController The Trajectory Tracker PID controller
+ * for angle for the robot.
+ * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
+ * @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing
+ * the output wheel speeds.
+ * @param requirements The subsystems to require.
+ */
+
+ @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
+ public MecanumControllerCommand(Trajectory trajectory,
+ Supplier<Pose2d> pose,
+ MecanumDriveKinematics kinematics,
+ PIDController xController,
+ PIDController yController,
+ ProfiledPIDController thetaController,
+
+ double maxWheelVelocityMetersPerSecond,
+
+ Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
+ Subsystem... requirements) {
+ m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
+ m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
+ m_feedforward = new SimpleMotorFeedforward(0, 0, 0);
+ m_kinematics = requireNonNullParam(kinematics,
+ "kinematics", "MecanumControllerCommand");
+
+ m_xController = requireNonNullParam(xController,
+ "xController", "MecanumControllerCommand");
+ m_yController = requireNonNullParam(yController,
+ "xController", "MecanumControllerCommand");
+ m_thetaController = requireNonNullParam(thetaController,
+ "thetaController", "MecanumControllerCommand");
+
+ m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond,
+ "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
+
+ m_frontLeftController = null;
+ m_rearLeftController = null;
+ m_frontRightController = null;
+ m_rearRightController = null;
+
+ m_currentWheelSpeeds = null;
+
+ m_outputWheelSpeeds = requireNonNullParam(outputWheelSpeeds,
+ "outputWheelSpeeds", "MecanumControllerCommand");
+
+ m_outputDriveVoltages = null;
+
+ m_usePID = false;
+
+ addRequirements(requirements);
+ }
+
+ @Override
+ public void initialize() {
+ var initialState = m_trajectory.sample(0);
+
+ // Sample final pose to get robot rotation
+ m_finalPose = m_trajectory.sample(m_trajectory.getTotalTimeSeconds()).poseMeters;
+
+ var initialXVelocity = initialState.velocityMetersPerSecond
+ * initialState.poseMeters.getRotation().getCos();
+ var initialYVelocity = initialState.velocityMetersPerSecond
+ * initialState.poseMeters.getRotation().getSin();
+
+ m_prevSpeeds = m_kinematics.toWheelSpeeds(
+ new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
+
+ m_timer.reset();
+ m_timer.start();
+ }
+
+ @Override
+ @SuppressWarnings("LocalVariableName")
+ public void execute() {
+ double curTime = m_timer.get();
+ double dt = curTime - m_prevTime;
+
+ var desiredState = m_trajectory.sample(curTime);
+ var desiredPose = desiredState.poseMeters;
+
+ var poseError = desiredPose.relativeTo(m_pose.get());
+
+ double targetXVel = m_xController.calculate(
+ m_pose.get().getTranslation().getX(),
+ desiredPose.getTranslation().getX());
+
+ double targetYVel = m_yController.calculate(
+ m_pose.get().getTranslation().getY(),
+ desiredPose.getTranslation().getY());
+
+ // The robot will go to the desired rotation of the final pose in the trajectory,
+ // not following the poses at individual states.
+ double targetAngularVel = m_thetaController.calculate(
+ m_pose.get().getRotation().getRadians(),
+ m_finalPose.getRotation().getRadians());
+
+ double vRef = desiredState.velocityMetersPerSecond;
+
+ targetXVel += vRef * poseError.getRotation().getCos();
+ targetYVel += vRef * poseError.getRotation().getSin();
+
+ var targetChassisSpeeds = new ChassisSpeeds(targetXVel, targetYVel, targetAngularVel);
+
+ var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
+
+ targetWheelSpeeds.normalize(m_maxWheelVelocityMetersPerSecond);
+
+ var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond;
+ var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond;
+ var frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond;
+ var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond;
+
+ double frontLeftOutput;
+ double rearLeftOutput;
+ double frontRightOutput;
+ double rearRightOutput;
+
+ if (m_usePID) {
+ final double frontLeftFeedforward = m_feedforward.calculate(frontLeftSpeedSetpoint,
+ (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
+
+ final double rearLeftFeedforward = m_feedforward.calculate(rearLeftSpeedSetpoint,
+ (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
+
+ final double frontRightFeedforward = m_feedforward.calculate(frontRightSpeedSetpoint,
+ (frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
+
+ final double rearRightFeedforward = m_feedforward.calculate(rearRightSpeedSetpoint,
+ (rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
+
+ frontLeftOutput = frontLeftFeedforward + m_frontLeftController.calculate(
+ m_currentWheelSpeeds.get().frontLeftMetersPerSecond,
+ frontLeftSpeedSetpoint);
+
+ rearLeftOutput = rearLeftFeedforward + m_rearLeftController.calculate(
+ m_currentWheelSpeeds.get().rearLeftMetersPerSecond,
+ rearLeftSpeedSetpoint);
+
+ frontRightOutput = frontRightFeedforward + m_frontRightController.calculate(
+ m_currentWheelSpeeds.get().frontRightMetersPerSecond,
+ frontRightSpeedSetpoint);
+
+ rearRightOutput = rearRightFeedforward + m_rearRightController.calculate(
+ m_currentWheelSpeeds.get().rearRightMetersPerSecond,
+ rearRightSpeedSetpoint);
+
+ m_outputDriveVoltages.accept(new MecanumDriveMotorVoltages(
+ frontLeftOutput,
+ frontRightOutput,
+ rearLeftOutput,
+ rearRightOutput));
+
+ } else {
+ m_outputWheelSpeeds.accept(new MecanumDriveWheelSpeeds(
+ frontLeftSpeedSetpoint,
+ frontRightSpeedSetpoint,
+ rearLeftSpeedSetpoint,
+ rearRightSpeedSetpoint));
+ }
+
+ m_prevTime = curTime;
+ m_prevSpeeds = targetWheelSpeeds;
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_timer.stop();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
similarity index 81%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
index 8bd8767..5f2b093 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
@@ -72,12 +72,13 @@
@Override
public void execute() {
- useOutput(m_controller.calculate(getMeasurement(), getSetpoint()));
+ m_useOutput.accept(m_controller.calculate(m_measurement.getAsDouble(),
+ m_setpoint.getAsDouble()));
}
@Override
public void end(boolean interrupted) {
- useOutput(0);
+ m_useOutput.accept(0);
}
/**
@@ -88,31 +89,4 @@
public PIDController getController() {
return m_controller;
}
-
- /**
- * Gets the setpoint for the controller. Wraps the passed-in function for readability.
- *
- * @return The setpoint for the controller
- */
- private double getSetpoint() {
- return m_setpoint.getAsDouble();
- }
-
- /**
- * Gets the measurement of the process variable. Wraps the passed-in function for readability.
- *
- * @return The measurement of the process variable
- */
- private double getMeasurement() {
- return m_measurement.getAsDouble();
- }
-
- /**
- * Uses the output of the controller. Wraps the passed-in function for readability.
- *
- * @param output The output value to use
- */
- private void useOutput(double output) {
- m_useOutput.accept(output);
- }
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
similarity index 74%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
index f853f25..575ebb4 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
@@ -19,6 +19,8 @@
protected final PIDController m_controller;
protected boolean m_enabled;
+ private double m_setpoint;
+
/**
* Creates a new PIDSubsystem.
*
@@ -32,7 +34,7 @@
@Override
public void periodic() {
if (m_enabled) {
- useOutput(m_controller.calculate(getMeasurement(), getSetpoint()));
+ useOutput(m_controller.calculate(getMeasurement(), m_setpoint), m_setpoint);
}
}
@@ -41,25 +43,28 @@
}
/**
+ * Sets the setpoint for the subsystem.
+ *
+ * @param setpoint the setpoint for the subsystem
+ */
+ public void setSetpoint(double setpoint) {
+ m_setpoint = setpoint;
+ }
+
+ /**
* Uses the output from the PIDController.
*
* @param output the output of the PIDController
+ * @param setpoint the setpoint of the PIDController (for feedforward)
*/
- public abstract void useOutput(double output);
-
- /**
- * Returns the reference (setpoint) used by the PIDController.
- *
- * @return the reference (setpoint) to be used by the controller
- */
- public abstract double getSetpoint();
+ protected abstract void useOutput(double output, double setpoint);
/**
* Returns the measurement of the process variable used by the PIDController.
*
* @return the measurement of the process variable
*/
- public abstract double getMeasurement();
+ protected abstract double getMeasurement();
/**
* Enables the PID control. Resets the controller.
@@ -74,6 +79,15 @@
*/
public void disable() {
m_enabled = false;
- useOutput(0);
+ useOutput(0, 0);
+ }
+
+ /**
+ * Returns whether the controller is enabled.
+ *
+ * @return Whether the controller is enabled.
+ */
+ public boolean isEnabled() {
+ return m_enabled;
}
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
similarity index 94%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
index 61517ef..37be702 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
@@ -21,6 +21,7 @@
//maps commands in this group to whether they are still running
private final Map<Command, Boolean> m_commands = new HashMap<>();
private boolean m_runWhenDisabled = true;
+ private boolean m_finished = true;
private Command m_deadline;
/**
@@ -57,7 +58,7 @@
public final void addCommands(Command... commands) {
requireUngrouped(commands);
- if (m_commands.containsValue(true)) {
+ if (!m_finished) {
throw new IllegalStateException(
"Commands cannot be added to a CommandGroup while the group is running");
}
@@ -81,6 +82,7 @@
commandRunning.getKey().initialize();
commandRunning.setValue(true);
}
+ m_finished = false;
}
@Override
@@ -93,6 +95,9 @@
if (commandRunning.getKey().isFinished()) {
commandRunning.getKey().end(false);
commandRunning.setValue(false);
+ if (commandRunning.getKey() == m_deadline) {
+ m_finished = true;
+ }
}
}
}
@@ -108,7 +113,7 @@
@Override
public boolean isFinished() {
- return m_deadline.isFinished();
+ return m_finished;
}
@Override
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
similarity index 87%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
index fcdb36b..076b4b8 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
@@ -118,12 +118,13 @@
@Override
public void execute() {
- useOutput(m_controller.calculate(getMeasurement(), getGoal()), m_controller.getSetpoint());
+ m_useOutput.accept(m_controller.calculate(m_measurement.getAsDouble(), m_goal.get()),
+ m_controller.getSetpoint());
}
@Override
public void end(boolean interrupted) {
- useOutput(0, new State());
+ m_useOutput.accept(0.0, new State());
}
/**
@@ -134,31 +135,4 @@
public ProfiledPIDController getController() {
return m_controller;
}
-
- /**
- * Gets the goal for the controller. Wraps the passed-in function for readability.
- *
- * @return The goal for the controller
- */
- private State getGoal() {
- return m_goal.get();
- }
-
- /**
- * Gets the measurement of the process variable. Wraps the passed-in function for readability.
- *
- * @return The measurement of the process variable
- */
- private double getMeasurement() {
- return m_measurement.getAsDouble();
- }
-
- /**
- * Uses the output of the controller. Wraps the passed-in function for readability.
- *
- * @param output The output value to use
- */
- private void useOutput(double output, State state) {
- m_useOutput.accept(output, state);
- }
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
similarity index 65%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
index ecd3371..6b3492d 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
@@ -8,6 +8,7 @@
package edu.wpi.first.wpilibj2.command;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
@@ -20,6 +21,8 @@
protected final ProfiledPIDController m_controller;
protected boolean m_enabled;
+ private TrapezoidProfile.State m_goal;
+
/**
* Creates a new ProfiledPIDSubsystem.
*
@@ -33,7 +36,7 @@
@Override
public void periodic() {
if (m_enabled) {
- useOutput(m_controller.calculate(getMeasurement(), getGoal()), m_controller.getSetpoint());
+ useOutput(m_controller.calculate(getMeasurement(), m_goal), m_controller.getSetpoint());
}
}
@@ -42,26 +45,37 @@
}
/**
- * Uses the output from the ProfiledPIDController.
+ * Sets the goal state for the subsystem.
*
- * @param output the output of the ProfiledPIDController
- * @param goal the goal state of the ProfiledPIDController, for feedforward
+ * @param goal The goal state for the subsystem's motion profile.
*/
- public abstract void useOutput(double output, State goal);
+ public void setGoal(TrapezoidProfile.State goal) {
+ m_goal = goal;
+ }
/**
- * Returns the goal used by the ProfiledPIDController.
+ * Sets the goal state for the subsystem. Goal velocity assumed to be zero.
*
- * @return the goal to be used by the controller
+ * @param goal The goal position for the subsystem's motion profile.
*/
- public abstract State getGoal();
+ public void setGoal(double goal) {
+ setGoal(new TrapezoidProfile.State(goal, 0));
+ }
+
+ /**
+ * Uses the output from the ProfiledPIDController.
+ *
+ * @param output the output of the ProfiledPIDController
+ * @param setpoint the setpoint state of the ProfiledPIDController, for feedforward
+ */
+ protected abstract void useOutput(double output, State setpoint);
/**
* Returns the measurement of the process variable used by the ProfiledPIDController.
*
* @return the measurement of the process variable
*/
- public abstract double getMeasurement();
+ protected abstract double getMeasurement();
/**
* Enables the PID control. Resets the controller.
@@ -78,4 +92,13 @@
m_enabled = false;
useOutput(0, new State());
}
+
+ /**
+ * Returns whether the controller is enabled.
+ *
+ * @return Whether the controller is enabled.
+ */
+ public boolean isEnabled() {
+ return m_enabled;
+ }
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
similarity index 65%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
index 754660c..eea06c7 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
@@ -8,12 +8,12 @@
package edu.wpi.first.wpilibj2.command;
import java.util.function.BiConsumer;
-import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.RamseteController;
+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;
@@ -35,63 +35,52 @@
* the PID and feedforward functionality, returning only the raw wheel speeds from the RAMSETE
* controller.
*/
+@SuppressWarnings("PMD.TooManyFields")
public class RamseteCommand extends CommandBase {
private final Timer m_timer = new Timer();
- private DifferentialDriveWheelSpeeds m_prevSpeeds;
- private double m_prevTime;
-
+ private final boolean m_usePID;
private final Trajectory m_trajectory;
private final Supplier<Pose2d> m_pose;
private final RamseteController m_follower;
- private final double m_ks;
- private final double m_kv;
- private final double m_ka;
+ private final SimpleMotorFeedforward m_feedforward;
private final DifferentialDriveKinematics m_kinematics;
- private final DoubleSupplier m_leftSpeed;
- private final DoubleSupplier m_rightSpeed;
+ private final Supplier<DifferentialDriveWheelSpeeds> m_speeds;
private final PIDController m_leftController;
private final PIDController m_rightController;
private final BiConsumer<Double, Double> m_output;
+ private DifferentialDriveWheelSpeeds m_prevSpeeds;
+ private double m_prevTime;
/**
* Constructs a new RamseteCommand that, when executed, will follow the provided trajectory.
- * PID control and feedforward are handled internally, and outputs are scaled -1 to 1 for easy
- * consumption by speed controllers.
+ * PID control and feedforward are handled internally, and outputs are scaled -12 to 12
+ * representing units of volts.
*
* <p>Note: The controller will *not* set the outputVolts to zero upon completion of the path -
* this
* is left to the user, since it is not appropriate for paths with nonstationary endstates.
*
- * @param trajectory The trajectory to follow.
- * @param pose A function that supplies the robot pose - use one of
- * the odometry classes to provide this.
- * @param controller The RAMSETE controller used to follow the trajectory.
- * @param ksVolts Constant feedforward term for the robot drive.
- * @param kvVoltSecondsPerMeter Velocity-proportional feedforward term for the robot
- * drive.
- * @param kaVoltSecondsSquaredPerMeter Acceleration-proportional feedforward term for the robot
- * drive.
- * @param kinematics The kinematics for the robot drivetrain.
- * @param leftWheelSpeedMetersPerSecond A function that supplies the speed of the left side of
- * the robot drive.
- * @param rightWheelSpeedMetersPerSecond A function that supplies the speed of the right side of
- * the robot drive.
- * @param leftController The PIDController for the left side of the robot drive.
- * @param rightController The PIDController for the right side of the robot drive.
- * @param outputVolts A function that consumes the computed left and right
- * outputs (in volts) for the robot drive.
- * @param requirements The subsystems to require.
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose - use one of
+ * the odometry classes to provide this.
+ * @param controller The RAMSETE controller used to follow the trajectory.
+ * @param feedforward The feedforward to use for the drive.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param wheelSpeeds A function that supplies the speeds of the left and
+ * right sides of the robot drive.
+ * @param leftController The PIDController for the left side of the robot drive.
+ * @param rightController The PIDController for the right side of the robot drive.
+ * @param outputVolts A function that consumes the computed left and right
+ * outputs (in volts) for the robot drive.
+ * @param requirements The subsystems to require.
*/
@SuppressWarnings("PMD.ExcessiveParameterList")
public RamseteCommand(Trajectory trajectory,
Supplier<Pose2d> pose,
RamseteController controller,
- double ksVolts,
- double kvVoltSecondsPerMeter,
- double kaVoltSecondsSquaredPerMeter,
+ SimpleMotorFeedforward feedforward,
DifferentialDriveKinematics kinematics,
- DoubleSupplier leftWheelSpeedMetersPerSecond,
- DoubleSupplier rightWheelSpeedMetersPerSecond,
+ Supplier<DifferentialDriveWheelSpeeds> wheelSpeeds,
PIDController leftController,
PIDController rightController,
BiConsumer<Double, Double> outputVolts,
@@ -99,20 +88,15 @@
m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand");
m_pose = requireNonNullParam(pose, "pose", "RamseteCommand");
m_follower = requireNonNullParam(controller, "controller", "RamseteCommand");
- m_ks = ksVolts;
- m_kv = kvVoltSecondsPerMeter;
- m_ka = kaVoltSecondsSquaredPerMeter;
+ m_feedforward = feedforward;
m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
- m_leftSpeed = requireNonNullParam(leftWheelSpeedMetersPerSecond,
- "leftWheelSpeedMetersPerSecond",
- "RamseteCommand");
- m_rightSpeed = requireNonNullParam(rightWheelSpeedMetersPerSecond,
- "rightWheelSpeedMetersPerSecond",
- "RamseteCommand");
+ m_speeds = requireNonNullParam(wheelSpeeds, "wheelSpeeds", "RamseteCommand");
m_leftController = requireNonNullParam(leftController, "leftController", "RamseteCommand");
m_rightController = requireNonNullParam(rightController, "rightController", "RamseteCommand");
m_output = requireNonNullParam(outputVolts, "outputVolts", "RamseteCommand");
+ m_usePID = true;
+
addRequirements(requirements);
}
@@ -142,14 +126,13 @@
m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
m_output = requireNonNullParam(outputMetersPerSecond, "output", "RamseteCommand");
- m_ks = 0;
- m_kv = 0;
- m_ka = 0;
- m_leftSpeed = null;
- m_rightSpeed = null;
+ m_feedforward = null;
+ m_speeds = null;
m_leftController = null;
m_rightController = null;
+ m_usePID = false;
+
addRequirements(requirements);
}
@@ -159,13 +142,15 @@
var initialState = m_trajectory.sample(0);
m_prevSpeeds = m_kinematics.toWheelSpeeds(
new ChassisSpeeds(initialState.velocityMetersPerSecond,
- 0,
- initialState.curvatureRadPerMeter
- * initialState.velocityMetersPerSecond));
+ 0,
+ initialState.curvatureRadPerMeter
+ * initialState.velocityMetersPerSecond));
m_timer.reset();
m_timer.start();
- m_leftController.reset();
- m_rightController.reset();
+ if (m_usePID) {
+ m_leftController.reset();
+ m_rightController.reset();
+ }
}
@Override
@@ -182,24 +167,22 @@
double leftOutput;
double rightOutput;
- if (m_leftController != null) {
+ if (m_usePID) {
double leftFeedforward =
- m_ks * Math.signum(leftSpeedSetpoint)
- + m_kv * leftSpeedSetpoint
- + m_ka * (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt;
+ m_feedforward.calculate(leftSpeedSetpoint,
+ (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt);
double rightFeedforward =
- m_ks * Math.signum(rightSpeedSetpoint)
- + m_kv * rightSpeedSetpoint
- + m_ka * (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt;
+ m_feedforward.calculate(rightSpeedSetpoint,
+ (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt);
leftOutput = leftFeedforward
- + m_leftController.calculate(m_leftSpeed.getAsDouble(),
- leftSpeedSetpoint);
+ + m_leftController.calculate(m_speeds.get().leftMetersPerSecond,
+ leftSpeedSetpoint);
rightOutput = rightFeedforward
- + m_rightController.calculate(m_rightSpeed.getAsDouble(),
- rightSpeedSetpoint);
+ + m_rightController.calculate(m_speeds.get().rightMetersPerSecond,
+ rightSpeedSetpoint);
} else {
leftOutput = leftSpeedSetpoint;
rightOutput = rightSpeedSetpoint;
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
new file mode 100644
index 0000000..b05dc20
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.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.wpilibj2.command;
+
+
+import java.util.function.Consumer;
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that uses two PID controllers ({@link PIDController}) and a
+ * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
+ * {@link Trajectory} with a swerve drive.
+ *
+ * <p>This command outputs the raw desired Swerve Module States ({@link SwerveModuleState})
+ * in an array. The desired wheel and module rotation velocities should be taken
+ * from those and used in velocity PIDs.
+ *
+ * <p>The robot angle controller does not follow the angle given by
+ * the trajectory but rather goes to the angle given in the final state of the trajectory.
+ */
+
+@SuppressWarnings("MemberName")
+public class SwerveControllerCommand extends CommandBase {
+ private final Timer m_timer = new Timer();
+ private Pose2d m_finalPose;
+
+ private final Trajectory m_trajectory;
+ private final Supplier<Pose2d> m_pose;
+ private final SwerveDriveKinematics m_kinematics;
+ private final PIDController m_xController;
+ private final PIDController m_yController;
+ private final ProfiledPIDController m_thetaController;
+ private final Consumer<SwerveModuleState[]> m_outputModuleStates;
+
+ /**
+ * Constructs a new SwerveControllerCommand that when executed will follow the provided
+ * trajectory. This command will not return output voltages but rather raw module states from the
+ * position controllers which need to be put into a velocity PID.
+ *
+ * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path-
+ * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
+ *
+ * <p>Note 2: The rotation controller will calculate the rotation based on the final pose
+ * in the trajectory, not the poses at each time step.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose - use one of
+ * the odometry classes to provide this.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param xController The Trajectory Tracker PID controller
+ * for the robot's x position.
+ * @param yController The Trajectory Tracker PID controller
+ * for the robot's y position.
+ * @param thetaController The Trajectory Tracker PID controller
+ * for angle for the robot.
+ * @param outputModuleStates The raw output module states from the
+ * position controllers.
+ * @param requirements The subsystems to require.
+ */
+
+ @SuppressWarnings("ParameterName")
+ public SwerveControllerCommand(Trajectory trajectory,
+ Supplier<Pose2d> pose,
+ SwerveDriveKinematics kinematics,
+ PIDController xController,
+ PIDController yController,
+ ProfiledPIDController thetaController,
+
+ Consumer<SwerveModuleState[]> outputModuleStates,
+ Subsystem... requirements) {
+ m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand");
+ m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand");
+ m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand");
+
+ m_xController = requireNonNullParam(xController,
+ "xController", "SwerveControllerCommand");
+ m_yController = requireNonNullParam(yController,
+ "xController", "SwerveControllerCommand");
+ m_thetaController = requireNonNullParam(thetaController,
+ "thetaController", "SwerveControllerCommand");
+
+ m_outputModuleStates = requireNonNullParam(outputModuleStates,
+ "frontLeftOutput", "SwerveControllerCommand");
+ addRequirements(requirements);
+ }
+
+ @Override
+ public void initialize() {
+ // Sample final pose to get robot rotation
+ m_finalPose = m_trajectory.sample(m_trajectory.getTotalTimeSeconds()).poseMeters;
+
+ m_timer.reset();
+ m_timer.start();
+ }
+
+ @Override
+ @SuppressWarnings("LocalVariableName")
+ public void execute() {
+ double curTime = m_timer.get();
+
+ var desiredState = m_trajectory.sample(curTime);
+ var desiredPose = desiredState.poseMeters;
+
+ var poseError = desiredPose.relativeTo(m_pose.get());
+
+ double targetXVel = m_xController.calculate(
+ m_pose.get().getTranslation().getX(),
+ desiredPose.getTranslation().getX());
+
+ double targetYVel = m_yController.calculate(
+ m_pose.get().getTranslation().getY(),
+ desiredPose.getTranslation().getY());
+
+ // The robot will go to the desired rotation of the final pose in the trajectory,
+ // not following the poses at individual states.
+ double targetAngularVel = m_thetaController.calculate(
+ m_pose.get().getRotation().getRadians(),
+ m_finalPose.getRotation().getRadians());
+
+ double vRef = desiredState.velocityMetersPerSecond;
+
+ targetXVel += vRef * poseError.getRotation().getCos();
+ targetYVel += vRef * poseError.getRotation().getSin();
+
+ var targetChassisSpeeds = new ChassisSpeeds(targetXVel, targetYVel, targetAngularVel);
+
+ var targetModuleStates = m_kinematics.toSwerveModuleStates(targetChassisSpeeds);
+
+ m_outputModuleStates.accept(targetModuleStates);
+
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_timer.stop();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
new file mode 100644
index 0000000..b7ec8c1
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.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.wpilibj2.command;
+
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+/**
+ * A subsystem that generates and runs trapezoidal motion profiles automatically. The user
+ * specifies how to use the current state of the motion profile by overriding the `useState` method.
+ */
+public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
+ private final double m_period;
+ private final TrapezoidProfile.Constraints m_constraints;
+
+ private TrapezoidProfile.State m_state;
+ private TrapezoidProfile.State m_goal;
+
+ /**
+ * Creates a new TrapezoidProfileSubsystem.
+ *
+ * @param constraints The constraints (maximum velocity and acceleration) for the profiles.
+ * @param initialPosition The initial position of the controller mechanism when the subsystem
+ * is constructed.
+ */
+ public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints,
+ double initialPosition) {
+ m_constraints = constraints;
+ m_state = new TrapezoidProfile.State(initialPosition, 0);
+ m_period = 0.02;
+ }
+
+ /**
+ * Creates a new TrapezoidProfileSubsystem.
+ *
+ * @param constraints The constraints (maximum velocity and acceleration) for the profiles.
+ * @param initialPosition The initial position of the controller mechanism when the subsystem
+ * is constructed.
+ * @param period The period of the main robot loop, in seconds.
+ */
+ public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints,
+ double initialPosition,
+ double period) {
+ m_constraints = constraints;
+ m_state = new TrapezoidProfile.State(initialPosition, 0);
+ m_period = period;
+ }
+
+ @Override
+ public void periodic() {
+ var profile = new TrapezoidProfile(m_constraints, m_goal, m_state);
+ m_state = profile.calculate(m_period);
+ useState(m_state);
+ }
+
+ /**
+ * Sets the goal state for the subsystem.
+ *
+ * @param goal The goal state for the subsystem's motion profile.
+ */
+ public void setGoal(TrapezoidProfile.State goal) {
+ m_goal = goal;
+ }
+
+ /**
+ * Sets the goal state for the subsystem. Goal velocity assumed to be zero.
+ *
+ * @param goal The goal position for the subsystem's motion profile.
+ */
+ public void setGoal(double goal) {
+ setGoal(new TrapezoidProfile.State(goal, 0));
+ }
+
+ /**
+ * Users should override this to consume the current state of the motion profile.
+ *
+ * @param state The current state of the motion profile.
+ */
+ protected abstract void useState(TrapezoidProfile.State state);
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
similarity index 90%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
index d4ff657..b7f26ae 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
@@ -10,6 +10,7 @@
import java.util.function.BooleanSupplier;
import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
/**
* This class provides an easy way to link commands to OI inputs.
@@ -66,11 +67,12 @@
/**
* Runs the given runnable whenever the button is newly pressed.
*
- * @param toRun the runnable to run
+ * @param toRun the runnable to run
+ * @param requirements the required subsystems
* @return this button, so calls can be chained
*/
- public Button whenPressed(final Runnable toRun) {
- whenActive(toRun);
+ public Button whenPressed(final Runnable toRun, Subsystem... requirements) {
+ whenActive(toRun, requirements);
return this;
}
@@ -106,11 +108,12 @@
/**
* Constantly runs the given runnable while the button is held.
*
- * @param toRun the runnable to run
+ * @param toRun the runnable to run
+ * @param requirements the required subsystems
* @return this button, so calls can be chained
*/
- public Button whileHeld(final Runnable toRun) {
- whileActiveContinuous(toRun);
+ public Button whileHeld(final Runnable toRun, Subsystem... requirements) {
+ whileActiveContinuous(toRun, requirements);
return this;
}
@@ -167,11 +170,12 @@
/**
* Runs the given runnable when the button is released.
*
- * @param toRun the runnable to run
+ * @param toRun the runnable to run
+ * @param requirements the required subsystems
* @return this button, so calls can be chained
*/
- public Button whenReleased(final Runnable toRun) {
- whenInactive(toRun);
+ public Button whenReleased(final Runnable toRun, Subsystem... requirements) {
+ whenInactive(toRun, requirements);
return this;
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
similarity index 92%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
rename to wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
index 9a12def..5167e4a 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
@@ -12,6 +12,7 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.Subsystem;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
@@ -100,11 +101,12 @@
/**
* Runs the given runnable whenever the trigger just becomes active.
*
- * @param toRun the runnable to run
+ * @param toRun the runnable to run
+ * @param requirements the required subsystems
* @return this trigger, so calls can be chained
*/
- public Trigger whenActive(final Runnable toRun) {
- return whenActive(new InstantCommand(toRun));
+ public Trigger whenActive(final Runnable toRun, Subsystem... requirements) {
+ return whenActive(new InstantCommand(toRun, requirements));
}
/**
@@ -155,11 +157,12 @@
/**
* Constantly runs the given runnable while the button is held.
*
- * @param toRun the runnable to run
+ * @param toRun the runnable to run
+ * @param requirements the required subsystems
* @return this trigger, so calls can be chained
*/
- public Trigger whileActiveContinuous(final Runnable toRun) {
- return whileActiveContinuous(new InstantCommand(toRun));
+ public Trigger whileActiveContinuous(final Runnable toRun, Subsystem... requirements) {
+ return whileActiveContinuous(new InstantCommand(toRun, requirements));
}
/**
@@ -243,11 +246,12 @@
/**
* Runs the given runnable when the trigger becomes inactive.
*
- * @param toRun the runnable to run
+ * @param toRun the runnable to run
+ * @param requirements the required subsystems
* @return this trigger, so calls can be chained
*/
- public Trigger whenInactive(final Runnable toRun) {
- return whenInactive(new InstantCommand(toRun));
+ public Trigger whenInactive(final Runnable toRun, Subsystem... requirements) {
+ return whenInactive(new InstantCommand(toRun, requirements));
}
/**
diff --git a/wpilibc/src/main/native/cpp/frc2/command/Command.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
similarity index 88%
rename from wpilibc/src/main/native/cpp/frc2/command/Command.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
index 692879c..83c9721 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/Command.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
@@ -7,8 +7,6 @@
#include "frc2/command/Command.h"
-#include <iostream>
-
#include "frc2/command/CommandScheduler.h"
#include "frc2/command/InstantCommand.h"
#include "frc2/command/ParallelCommandGroup.h"
@@ -50,19 +48,23 @@
return ParallelRaceGroup(std::move(temp));
}
-SequentialCommandGroup Command::BeforeStarting(std::function<void()> toRun) && {
+SequentialCommandGroup Command::BeforeStarting(
+ std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements) && {
std::vector<std::unique_ptr<Command>> temp;
- temp.emplace_back(std::make_unique<InstantCommand>(
- std::move(toRun), std::initializer_list<Subsystem*>{}));
+ temp.emplace_back(
+ std::make_unique<InstantCommand>(std::move(toRun), requirements));
temp.emplace_back(std::move(*this).TransferOwnership());
return SequentialCommandGroup(std::move(temp));
}
-SequentialCommandGroup Command::AndThen(std::function<void()> toRun) && {
+SequentialCommandGroup Command::AndThen(
+ std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements) && {
std::vector<std::unique_ptr<Command>> temp;
temp.emplace_back(std::move(*this).TransferOwnership());
- temp.emplace_back(std::make_unique<InstantCommand>(
- std::move(toRun), std::initializer_list<Subsystem*>{}));
+ temp.emplace_back(
+ std::make_unique<InstantCommand>(std::move(toRun), requirements));
return SequentialCommandGroup(std::move(temp));
}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
similarity index 96%
rename from wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
index aeba26b..323ed67 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
@@ -9,8 +9,6 @@
#include <frc/smartdashboard/SendableBuilder.h>
#include <frc/smartdashboard/SendableRegistry.h>
-#include <frc2/command/CommandScheduler.h>
-#include <frc2/command/SetUtilities.h>
using namespace frc2;
diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
similarity index 80%
rename from wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
index ba53397..8869d4c 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
@@ -7,19 +7,10 @@
#include "frc2/command/CommandGroupBase.h"
-#include <set>
-
-#include "frc/WPIErrors.h"
-#include "frc2/command/ParallelCommandGroup.h"
-#include "frc2/command/ParallelDeadlineGroup.h"
-#include "frc2/command/ParallelRaceGroup.h"
-#include "frc2/command/SequentialCommandGroup.h"
+#include <frc/WPIErrors.h>
using namespace frc2;
-template <typename TMap, typename TKey>
-static bool ContainsKey(const TMap& map, TKey keyToCheck) {
- return map.find(keyToCheck) != map.end();
-}
+
bool CommandGroupBase::RequireUngrouped(Command& command) {
if (command.IsGrouped()) {
wpi_setGlobalWPIErrorWithContext(
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
new file mode 100644
index 0000000..84d0820
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
@@ -0,0 +1,399 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandScheduler.h"
+
+#include <frc/RobotState.h>
+#include <frc/WPIErrors.h>
+#include <frc/smartdashboard/SendableBuilder.h>
+#include <frc/smartdashboard/SendableRegistry.h>
+#include <hal/HALBase.h>
+#include <networktables/NetworkTableEntry.h>
+#include <wpi/DenseMap.h>
+
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandState.h"
+#include "frc2/command/Subsystem.h"
+
+using namespace frc2;
+
+class CommandScheduler::Impl {
+ public:
+ // A map from commands to their scheduling state. Also used as a set of the
+ // currently-running commands.
+ wpi::DenseMap<Command*, CommandState> scheduledCommands;
+
+ // A map from required subsystems to their requiring commands. Also used as a
+ // set of the currently-required subsystems.
+ wpi::DenseMap<Subsystem*, Command*> requirements;
+
+ // A map from subsystems registered with the scheduler to their default
+ // commands. Also used as a list of currently-registered subsystems.
+ wpi::DenseMap<Subsystem*, std::unique_ptr<Command>> subsystems;
+
+ // The set of currently-registered buttons that will be polled every
+ // iteration.
+ wpi::SmallVector<wpi::unique_function<void()>, 4> buttons;
+
+ bool disabled{false};
+
+ // NetworkTable entries for use in Sendable impl
+ nt::NetworkTableEntry namesEntry;
+ nt::NetworkTableEntry idsEntry;
+ nt::NetworkTableEntry cancelEntry;
+
+ // Lists of user-supplied actions to be executed on scheduling events for
+ // every command.
+ wpi::SmallVector<Action, 4> initActions;
+ wpi::SmallVector<Action, 4> executeActions;
+ wpi::SmallVector<Action, 4> interruptActions;
+ wpi::SmallVector<Action, 4> finishActions;
+
+ // Flag and queues for avoiding concurrent modification if commands are
+ // scheduled/canceled during run
+
+ bool inRunLoop = false;
+ wpi::DenseMap<Command*, bool> toSchedule;
+ wpi::SmallVector<Command*, 4> toCancel;
+};
+
+template <typename TMap, typename TKey>
+static bool ContainsKey(const TMap& map, TKey keyToCheck) {
+ return map.find(keyToCheck) != map.end();
+}
+
+CommandScheduler::CommandScheduler() : m_impl(new Impl) {
+ frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler");
+}
+
+CommandScheduler::~CommandScheduler() {}
+
+CommandScheduler& CommandScheduler::GetInstance() {
+ static CommandScheduler scheduler;
+ return scheduler;
+}
+
+void CommandScheduler::AddButton(wpi::unique_function<void()> button) {
+ m_impl->buttons.emplace_back(std::move(button));
+}
+
+void CommandScheduler::ClearButtons() { m_impl->buttons.clear(); }
+
+void CommandScheduler::Schedule(bool interruptible, Command* command) {
+ if (m_impl->inRunLoop) {
+ m_impl->toSchedule.try_emplace(command, interruptible);
+ return;
+ }
+
+ if (command->IsGrouped()) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "A command that is part of a command group "
+ "cannot be independently scheduled");
+ return;
+ }
+ if (m_impl->disabled ||
+ (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) ||
+ ContainsKey(m_impl->scheduledCommands, command)) {
+ return;
+ }
+
+ const auto& requirements = command->GetRequirements();
+
+ wpi::SmallVector<Command*, 8> intersection;
+
+ bool isDisjoint = true;
+ bool allInterruptible = true;
+ for (auto&& i1 : m_impl->requirements) {
+ if (requirements.find(i1.first) != requirements.end()) {
+ isDisjoint = false;
+ allInterruptible &=
+ m_impl->scheduledCommands[i1.second].IsInterruptible();
+ intersection.emplace_back(i1.second);
+ }
+ }
+
+ if (isDisjoint || allInterruptible) {
+ if (allInterruptible) {
+ for (auto&& cmdToCancel : intersection) {
+ Cancel(cmdToCancel);
+ }
+ }
+ command->Initialize();
+ m_impl->scheduledCommands[command] = CommandState{interruptible};
+ for (auto&& action : m_impl->initActions) {
+ action(*command);
+ }
+ for (auto&& requirement : requirements) {
+ m_impl->requirements[requirement] = command;
+ }
+ }
+}
+
+void CommandScheduler::Schedule(Command* command) { Schedule(true, command); }
+
+void CommandScheduler::Schedule(bool interruptible,
+ wpi::ArrayRef<Command*> commands) {
+ for (auto command : commands) {
+ Schedule(interruptible, command);
+ }
+}
+
+void CommandScheduler::Schedule(bool interruptible,
+ std::initializer_list<Command*> commands) {
+ for (auto command : commands) {
+ Schedule(interruptible, command);
+ }
+}
+
+void CommandScheduler::Schedule(wpi::ArrayRef<Command*> commands) {
+ for (auto command : commands) {
+ Schedule(true, command);
+ }
+}
+
+void CommandScheduler::Schedule(std::initializer_list<Command*> commands) {
+ for (auto command : commands) {
+ Schedule(true, command);
+ }
+}
+
+void CommandScheduler::Run() {
+ if (m_impl->disabled) {
+ return;
+ }
+
+ // Run the periodic method of all registered subsystems.
+ for (auto&& subsystem : m_impl->subsystems) {
+ subsystem.getFirst()->Periodic();
+ }
+
+ // Poll buttons for new commands to add.
+ for (auto&& button : m_impl->buttons) {
+ button();
+ }
+
+ m_impl->inRunLoop = true;
+ // Run scheduled commands, remove finished commands.
+ for (auto iterator = m_impl->scheduledCommands.begin();
+ iterator != m_impl->scheduledCommands.end(); iterator++) {
+ Command* command = iterator->getFirst();
+
+ if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) {
+ Cancel(command);
+ continue;
+ }
+
+ command->Execute();
+ for (auto&& action : m_impl->executeActions) {
+ action(*command);
+ }
+
+ if (command->IsFinished()) {
+ command->End(false);
+ for (auto&& action : m_impl->finishActions) {
+ action(*command);
+ }
+
+ for (auto&& requirement : command->GetRequirements()) {
+ m_impl->requirements.erase(requirement);
+ }
+
+ m_impl->scheduledCommands.erase(iterator);
+ }
+ }
+ m_impl->inRunLoop = false;
+
+ for (auto&& commandInterruptible : m_impl->toSchedule) {
+ Schedule(commandInterruptible.second, commandInterruptible.first);
+ }
+
+ for (auto&& command : m_impl->toCancel) {
+ Cancel(command);
+ }
+
+ m_impl->toSchedule.clear();
+ m_impl->toCancel.clear();
+
+ // Add default commands for un-required registered subsystems.
+ for (auto&& subsystem : m_impl->subsystems) {
+ auto s = m_impl->requirements.find(subsystem.getFirst());
+ if (s == m_impl->requirements.end() && subsystem.getSecond()) {
+ Schedule({subsystem.getSecond().get()});
+ }
+ }
+}
+
+void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) {
+ m_impl->subsystems[subsystem] = nullptr;
+}
+
+void CommandScheduler::UnregisterSubsystem(Subsystem* subsystem) {
+ auto s = m_impl->subsystems.find(subsystem);
+ if (s != m_impl->subsystems.end()) {
+ m_impl->subsystems.erase(s);
+ }
+}
+
+void CommandScheduler::RegisterSubsystem(
+ std::initializer_list<Subsystem*> subsystems) {
+ for (auto* subsystem : subsystems) {
+ RegisterSubsystem(subsystem);
+ }
+}
+
+void CommandScheduler::UnregisterSubsystem(
+ std::initializer_list<Subsystem*> subsystems) {
+ for (auto* subsystem : subsystems) {
+ UnregisterSubsystem(subsystem);
+ }
+}
+
+Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
+ auto&& find = m_impl->subsystems.find(subsystem);
+ if (find != m_impl->subsystems.end()) {
+ return find->second.get();
+ } else {
+ return nullptr;
+ }
+}
+
+void CommandScheduler::Cancel(Command* command) {
+ if (m_impl->inRunLoop) {
+ m_impl->toCancel.emplace_back(command);
+ return;
+ }
+
+ auto find = m_impl->scheduledCommands.find(command);
+ if (find == m_impl->scheduledCommands.end()) return;
+ command->End(true);
+ for (auto&& action : m_impl->interruptActions) {
+ action(*command);
+ }
+ m_impl->scheduledCommands.erase(find);
+ for (auto&& requirement : m_impl->requirements) {
+ if (requirement.second == command) {
+ m_impl->requirements.erase(requirement.first);
+ }
+ }
+}
+
+void CommandScheduler::Cancel(wpi::ArrayRef<Command*> commands) {
+ for (auto command : commands) {
+ Cancel(command);
+ }
+}
+
+void CommandScheduler::Cancel(std::initializer_list<Command*> commands) {
+ for (auto command : commands) {
+ Cancel(command);
+ }
+}
+
+void CommandScheduler::CancelAll() {
+ for (auto&& command : m_impl->scheduledCommands) {
+ Cancel(command.first);
+ }
+}
+
+double CommandScheduler::TimeSinceScheduled(const Command* command) const {
+ auto find = m_impl->scheduledCommands.find(command);
+ if (find != m_impl->scheduledCommands.end()) {
+ return find->second.TimeSinceInitialized();
+ } else {
+ return -1;
+ }
+}
+bool CommandScheduler::IsScheduled(
+ wpi::ArrayRef<const Command*> commands) const {
+ for (auto command : commands) {
+ if (!IsScheduled(command)) {
+ return false;
+ }
+ }
+ return true;
+}
+
+bool CommandScheduler::IsScheduled(
+ std::initializer_list<const Command*> commands) const {
+ for (auto command : commands) {
+ if (!IsScheduled(command)) {
+ return false;
+ }
+ }
+ return true;
+}
+
+bool CommandScheduler::IsScheduled(const Command* command) const {
+ return m_impl->scheduledCommands.find(command) !=
+ m_impl->scheduledCommands.end();
+}
+
+Command* CommandScheduler::Requiring(const Subsystem* subsystem) const {
+ auto find = m_impl->requirements.find(subsystem);
+ if (find != m_impl->requirements.end()) {
+ return find->second;
+ } else {
+ return nullptr;
+ }
+}
+
+void CommandScheduler::Disable() { m_impl->disabled = true; }
+
+void CommandScheduler::Enable() { m_impl->disabled = false; }
+
+void CommandScheduler::OnCommandInitialize(Action action) {
+ m_impl->initActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandExecute(Action action) {
+ m_impl->executeActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandInterrupt(Action action) {
+ m_impl->interruptActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandFinish(Action action) {
+ m_impl->finishActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Scheduler");
+ m_impl->namesEntry = builder.GetEntry("Names");
+ m_impl->idsEntry = builder.GetEntry("Ids");
+ m_impl->cancelEntry = builder.GetEntry("Cancel");
+
+ builder.SetUpdateTable([this] {
+ double tmp[1];
+ tmp[0] = 0;
+ auto toCancel = m_impl->cancelEntry.GetDoubleArray(tmp);
+ for (auto cancel : toCancel) {
+ uintptr_t ptrTmp = static_cast<uintptr_t>(cancel);
+ Command* command = reinterpret_cast<Command*>(ptrTmp);
+ if (m_impl->scheduledCommands.find(command) !=
+ m_impl->scheduledCommands.end()) {
+ Cancel(command);
+ }
+ m_impl->cancelEntry.SetDoubleArray(wpi::ArrayRef<double>{});
+ }
+
+ wpi::SmallVector<std::string, 8> names;
+ wpi::SmallVector<double, 8> ids;
+ for (auto&& command : m_impl->scheduledCommands) {
+ names.emplace_back(command.first->GetName());
+ uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command.first);
+ ids.emplace_back(static_cast<double>(ptrTmp));
+ }
+ m_impl->namesEntry.SetStringArray(names);
+ m_impl->idsEntry.SetDoubleArray(ids);
+ });
+}
+
+void CommandScheduler::SetDefaultCommandImpl(Subsystem* subsystem,
+ std::unique_ptr<Command> command) {
+ m_impl->subsystems[subsystem] = std::move(command);
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp
similarity index 97%
rename from wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp
index 78ae006..b41fa70 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp
@@ -7,7 +7,7 @@
#include "frc2/command/CommandState.h"
-#include "frc/Timer.h"
+#include <frc/Timer.h>
using namespace frc2;
CommandState::CommandState(bool interruptible)
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ConditionalCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
similarity index 100%
rename from wpilibc/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
diff --git a/wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
similarity index 73%
rename from wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
index 63c3179..ef7c40a 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
@@ -9,14 +9,16 @@
using namespace frc2;
-FunctionalCommand::FunctionalCommand(std::function<void()> onInit,
- std::function<void()> onExecute,
- std::function<void(bool)> onEnd,
- std::function<bool()> isFinished)
+FunctionalCommand::FunctionalCommand(
+ std::function<void()> onInit, std::function<void()> onExecute,
+ std::function<void(bool)> onEnd, std::function<bool()> isFinished,
+ std::initializer_list<Subsystem*> requirements)
: m_onInit{std::move(onInit)},
m_onExecute{std::move(onExecute)},
m_onEnd{std::move(onEnd)},
- m_isFinished{std::move(isFinished)} {}
+ m_isFinished{std::move(isFinished)} {
+ AddRequirements(requirements);
+}
void FunctionalCommand::Initialize() { m_onInit(); }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/InstantCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp
similarity index 100%
rename from wpilibc/src/main/native/cpp/frc2/command/InstantCommand.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
new file mode 100644
index 0000000..15fb254
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
@@ -0,0 +1,187 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/MecanumControllerCommand.h"
+
+using namespace frc2;
+using namespace units;
+
+MecanumControllerCommand::MecanumControllerCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::SimpleMotorFeedforward<units::meters> feedforward,
+ frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+ frc2::PIDController yController,
+ frc::ProfiledPIDController<units::radians> thetaController,
+ units::meters_per_second_t maxWheelVelocity,
+ std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
+ frc2::PIDController frontLeftController,
+ frc2::PIDController rearLeftController,
+ frc2::PIDController frontRightController,
+ frc2::PIDController rearRightController,
+ std::function<void(units::volt_t, units::volt_t, units::volt_t,
+ units::volt_t)>
+ output,
+ std::initializer_list<Subsystem*> requirements)
+ : m_trajectory(trajectory),
+ m_pose(pose),
+ m_feedforward(feedforward),
+ m_kinematics(kinematics),
+ m_xController(std::make_unique<frc2::PIDController>(xController)),
+ m_yController(std::make_unique<frc2::PIDController>(yController)),
+ m_thetaController(
+ std::make_unique<frc::ProfiledPIDController<units::radians>>(
+ thetaController)),
+ m_maxWheelVelocity(maxWheelVelocity),
+ m_frontLeftController(
+ std::make_unique<frc2::PIDController>(frontLeftController)),
+ m_rearLeftController(
+ std::make_unique<frc2::PIDController>(rearLeftController)),
+ m_frontRightController(
+ std::make_unique<frc2::PIDController>(frontRightController)),
+ m_rearRightController(
+ std::make_unique<frc2::PIDController>(rearRightController)),
+ m_currentWheelSpeeds(currentWheelSpeeds),
+ m_outputVolts(output),
+ m_usePID(true) {
+ AddRequirements(requirements);
+}
+
+MecanumControllerCommand::MecanumControllerCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+ frc2::PIDController yController,
+ frc::ProfiledPIDController<units::radians> thetaController,
+ units::meters_per_second_t maxWheelVelocity,
+ std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+ units::meters_per_second_t, units::meters_per_second_t)>
+ output,
+ std::initializer_list<Subsystem*> requirements)
+ : m_trajectory(trajectory),
+ m_pose(pose),
+ m_kinematics(kinematics),
+ m_xController(std::make_unique<frc2::PIDController>(xController)),
+ m_yController(std::make_unique<frc2::PIDController>(yController)),
+ m_thetaController(
+ std::make_unique<frc::ProfiledPIDController<units::radians>>(
+ thetaController)),
+ m_maxWheelVelocity(maxWheelVelocity),
+ m_outputVel(output),
+ m_usePID(false) {
+ AddRequirements(requirements);
+}
+
+void MecanumControllerCommand::Initialize() {
+ m_prevTime = 0_s;
+ auto initialState = m_trajectory.Sample(0_s);
+
+ auto initialXVelocity =
+ initialState.velocity * initialState.pose.Rotation().Cos();
+ auto initialYVelocity =
+ initialState.velocity * initialState.pose.Rotation().Sin();
+
+ m_prevSpeeds = m_kinematics.ToWheelSpeeds(
+ frc::ChassisSpeeds{initialXVelocity, initialYVelocity, 0_rad_per_s});
+
+ m_timer.Reset();
+ m_timer.Start();
+ if (m_usePID) {
+ m_frontLeftController->Reset();
+ m_rearLeftController->Reset();
+ m_frontRightController->Reset();
+ m_rearRightController->Reset();
+ }
+}
+
+void MecanumControllerCommand::Execute() {
+ auto curTime = second_t(m_timer.Get());
+ auto dt = curTime - m_prevTime;
+
+ auto m_desiredState = m_trajectory.Sample(curTime);
+ auto m_desiredPose = m_desiredState.pose;
+
+ auto m_poseError = m_desiredPose.RelativeTo(m_pose());
+
+ auto targetXVel = meters_per_second_t(
+ m_xController->Calculate((m_pose().Translation().X().to<double>()),
+ (m_desiredPose.Translation().X().to<double>())));
+ auto targetYVel = meters_per_second_t(
+ m_yController->Calculate((m_pose().Translation().Y().to<double>()),
+ (m_desiredPose.Translation().Y().to<double>())));
+
+ // Profiled PID Controller only takes meters as setpoint and measurement
+ // The robot will go to the desired rotation of the final pose in the
+ // trajectory, not following the poses at individual states.
+ auto targetAngularVel = radians_per_second_t(m_thetaController->Calculate(
+ m_pose().Rotation().Radians(), m_finalPose.Rotation().Radians()));
+
+ auto vRef = m_desiredState.velocity;
+
+ targetXVel += vRef * m_poseError.Rotation().Cos();
+ targetYVel += vRef * m_poseError.Rotation().Sin();
+
+ auto targetChassisSpeeds =
+ frc::ChassisSpeeds{targetXVel, targetYVel, targetAngularVel};
+
+ auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(targetChassisSpeeds);
+
+ targetWheelSpeeds.Normalize(m_maxWheelVelocity);
+
+ auto frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft;
+ auto rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft;
+ auto frontRightSpeedSetpoint = targetWheelSpeeds.frontRight;
+ auto rearRightSpeedSetpoint = targetWheelSpeeds.rearRight;
+
+ if (m_usePID) {
+ auto frontLeftFeedforward = m_feedforward.Calculate(
+ frontLeftSpeedSetpoint,
+ (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeft) / dt);
+
+ auto rearLeftFeedforward = m_feedforward.Calculate(
+ rearLeftSpeedSetpoint,
+ (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeft) / dt);
+
+ auto frontRightFeedforward = m_feedforward.Calculate(
+ frontRightSpeedSetpoint,
+ (frontRightSpeedSetpoint - m_prevSpeeds.frontRight) / dt);
+
+ auto rearRightFeedforward = m_feedforward.Calculate(
+ rearRightSpeedSetpoint,
+ (rearRightSpeedSetpoint - m_prevSpeeds.rearRight) / dt);
+
+ auto frontLeftOutput = volt_t(m_frontLeftController->Calculate(
+ m_currentWheelSpeeds().frontLeft.to<double>(),
+ frontLeftSpeedSetpoint.to<double>())) +
+ frontLeftFeedforward;
+ auto rearLeftOutput = volt_t(m_rearLeftController->Calculate(
+ m_currentWheelSpeeds().rearLeft.to<double>(),
+ rearLeftSpeedSetpoint.to<double>())) +
+ rearLeftFeedforward;
+ auto frontRightOutput = volt_t(m_frontRightController->Calculate(
+ m_currentWheelSpeeds().frontRight.to<double>(),
+ frontRightSpeedSetpoint.to<double>())) +
+ frontRightFeedforward;
+ auto rearRightOutput = volt_t(m_rearRightController->Calculate(
+ m_currentWheelSpeeds().rearRight.to<double>(),
+ rearRightSpeedSetpoint.to<double>())) +
+ rearRightFeedforward;
+
+ m_outputVolts(frontLeftOutput, rearLeftOutput, frontRightOutput,
+ rearRightOutput);
+ } else {
+ m_outputVel(frontLeftSpeedSetpoint, rearLeftSpeedSetpoint,
+ frontRightSpeedSetpoint, rearRightSpeedSetpoint);
+
+ m_prevTime = curTime;
+ m_prevSpeeds = targetWheelSpeeds;
+ }
+}
+
+void MecanumControllerCommand::End(bool interrupted) { m_timer.Stop(); }
+
+bool MecanumControllerCommand::IsFinished() {
+ return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/NotifierCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp
similarity index 100%
rename from wpilibc/src/main/native/cpp/frc2/command/NotifierCommand.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
similarity index 67%
rename from wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
index 4391e60..4a956f6 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
@@ -31,29 +31,9 @@
void PIDCommand::Initialize() { m_controller.Reset(); }
void PIDCommand::Execute() {
- UseOutput(m_controller.Calculate(GetMeasurement(), m_setpoint()));
+ m_useOutput(m_controller.Calculate(m_measurement(), m_setpoint()));
}
-void PIDCommand::End(bool interrupted) { UseOutput(0); }
+void PIDCommand::End(bool interrupted) { m_useOutput(0); }
-void PIDCommand::SetOutput(std::function<void(double)> useOutput) {
- m_useOutput = useOutput;
-}
-
-void PIDCommand::SetSetpoint(std::function<double()> setpointSource) {
- m_setpoint = setpointSource;
-}
-
-void PIDCommand::SetSetpoint(double setpoint) {
- m_setpoint = [setpoint] { return setpoint; };
-}
-
-void PIDCommand::SetSetpointRelative(double relativeSetpoint) {
- SetSetpoint(m_setpoint() + relativeSetpoint);
-}
-
-double PIDCommand::GetMeasurement() { return m_measurement(); }
-
-void PIDCommand::UseOutput(double output) { m_useOutput(output); }
-
-PIDController& PIDCommand::getController() { return m_controller; }
+PIDController& PIDCommand::GetController() { return m_controller; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
similarity index 79%
rename from wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
index b81f6f6..ce8198a 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
@@ -14,18 +14,22 @@
void PIDSubsystem::Periodic() {
if (m_enabled) {
- UseOutput(m_controller.Calculate(GetMeasurement(), GetSetpoint()));
+ UseOutput(m_controller.Calculate(GetMeasurement(), m_setpoint), m_setpoint);
}
}
+void PIDSubsystem::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
+
void PIDSubsystem::Enable() {
m_controller.Reset();
m_enabled = true;
}
void PIDSubsystem::Disable() {
- UseOutput(0);
+ UseOutput(0, 0);
m_enabled = false;
}
+bool PIDSubsystem::IsEnabled() { return m_enabled; }
+
PIDController& PIDSubsystem::GetController() { return m_controller; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
similarity index 97%
rename from wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
index d8a4159..557984e 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
@@ -72,7 +72,7 @@
command->SetGrouped(true);
AddRequirements(command->GetRequirements());
m_runWhenDisabled &= command->RunsWhenDisabled();
- m_commands[std::move(command)] = false;
+ m_commands.emplace_back(std::move(command), false);
} else {
wpi_setWPIErrorWithContext(CommandIllegalUse,
"Multiple commands in a parallel group cannot "
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
similarity index 89%
rename from wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
index d967e67..935cf8b 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
@@ -21,7 +21,7 @@
commandRunning.first->Initialize();
commandRunning.second = true;
}
- isRunning = true;
+ m_finished = false;
}
void ParallelDeadlineGroup::Execute() {
@@ -31,6 +31,9 @@
if (commandRunning.first->IsFinished()) {
commandRunning.first->End(false);
commandRunning.second = false;
+ if (commandRunning.first.get() == m_deadline) {
+ m_finished = true;
+ }
}
}
}
@@ -41,10 +44,9 @@
commandRunning.first->End(true);
}
}
- isRunning = false;
}
-bool ParallelDeadlineGroup::IsFinished() { return m_deadline->IsFinished(); }
+bool ParallelDeadlineGroup::IsFinished() { return m_finished; }
bool ParallelDeadlineGroup::RunsWhenDisabled() const {
return m_runWhenDisabled;
@@ -56,7 +58,7 @@
return;
}
- if (isRunning) {
+ if (!m_finished) {
wpi_setWPIErrorWithContext(CommandIllegalUse,
"Commands cannot be added to a CommandGroup "
"while the group is running");
@@ -67,7 +69,7 @@
command->SetGrouped(true);
AddRequirements(command->GetRequirements());
m_runWhenDisabled &= command->RunsWhenDisabled();
- m_commands[std::move(command)] = false;
+ m_commands.emplace_back(std::move(command), false);
} else {
wpi_setWPIErrorWithContext(CommandIllegalUse,
"Multiple commands in a parallel group cannot "
@@ -80,7 +82,7 @@
void ParallelDeadlineGroup::SetDeadline(std::unique_ptr<Command>&& deadline) {
m_deadline = deadline.get();
m_deadline->SetGrouped(true);
- m_commands[std::move(deadline)] = false;
+ m_commands.emplace_back(std::move(deadline), false);
AddRequirements(m_deadline->GetRequirements());
m_runWhenDisabled &= m_deadline->RunsWhenDisabled();
}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
similarity index 97%
rename from wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
index 8a02717..466cc79 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
@@ -58,7 +58,7 @@
command->SetGrouped(true);
AddRequirements(command->GetRequirements());
m_runWhenDisabled &= command->RunsWhenDisabled();
- m_commands.emplace(std::move(command));
+ m_commands.emplace_back(std::move(command));
} else {
wpi_setWPIErrorWithContext(CommandIllegalUse,
"Multiple commands in a parallel group cannot "
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp
similarity index 100%
rename from wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp
similarity index 95%
rename from wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp
index 8bd62ea..e077760 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp
@@ -7,6 +7,8 @@
#include "frc2/command/PrintCommand.h"
+#include <wpi/raw_ostream.h>
+
using namespace frc2;
PrintCommand::PrintCommand(const wpi::Twine& message)
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp
similarity index 100%
rename from wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp
diff --git a/wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
similarity index 69%
rename from wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
index 0de739e..261896f 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
@@ -10,34 +10,25 @@
using namespace frc2;
using namespace units;
-template <typename T>
-int sgn(T val) {
- return (T(0) < val) - (val < T(0));
-}
-
RamseteCommand::RamseteCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
- frc::RamseteController controller, volt_t ks,
- units::unit_t<voltsecondspermeter> kv,
- units::unit_t<voltsecondssquaredpermeter> ka,
+ frc::RamseteController controller,
+ frc::SimpleMotorFeedforward<units::meters> feedforward,
frc::DifferentialDriveKinematics kinematics,
- std::function<units::meters_per_second_t()> leftSpeed,
- std::function<units::meters_per_second_t()> rightSpeed,
+ std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
frc2::PIDController leftController, frc2::PIDController rightController,
std::function<void(volt_t, volt_t)> output,
std::initializer_list<Subsystem*> requirements)
: m_trajectory(trajectory),
m_pose(pose),
m_controller(controller),
- m_ks(ks),
- m_kv(kv),
- m_ka(ka),
+ m_feedforward(feedforward),
m_kinematics(kinematics),
- m_leftSpeed(leftSpeed),
- m_rightSpeed(rightSpeed),
+ m_speeds(wheelSpeeds),
m_leftController(std::make_unique<frc2::PIDController>(leftController)),
m_rightController(std::make_unique<frc2::PIDController>(rightController)),
- m_outputVolts(output) {
+ m_outputVolts(output),
+ m_usePID(true) {
AddRequirements(requirements);
}
@@ -51,11 +42,9 @@
: m_trajectory(trajectory),
m_pose(pose),
m_controller(controller),
- m_ks(0),
- m_kv(0),
- m_ka(0),
m_kinematics(kinematics),
- m_outputVel(output) {
+ m_outputVel(output),
+ m_usePID(false) {
AddRequirements(requirements);
}
@@ -67,8 +56,10 @@
initialState.velocity * initialState.curvature});
m_timer.Reset();
m_timer.Start();
- m_leftController->Reset();
- m_rightController->Reset();
+ if (m_usePID) {
+ m_leftController->Reset();
+ m_rightController->Reset();
+ }
}
void RamseteCommand::Execute() {
@@ -78,22 +69,22 @@
auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(
m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime)));
- if (m_leftController.get() != nullptr) {
- auto leftFeedforward =
- m_ks * sgn(targetWheelSpeeds.left) + m_kv * targetWheelSpeeds.left +
- m_ka * (targetWheelSpeeds.left - m_prevSpeeds.left) / dt;
+ if (m_usePID) {
+ auto leftFeedforward = m_feedforward.Calculate(
+ targetWheelSpeeds.left,
+ (targetWheelSpeeds.left - m_prevSpeeds.left) / dt);
- auto rightFeedforward =
- m_ks * sgn(targetWheelSpeeds.right) + m_kv * targetWheelSpeeds.right +
- m_ka * (targetWheelSpeeds.right - m_prevSpeeds.right) / dt;
+ auto rightFeedforward = m_feedforward.Calculate(
+ targetWheelSpeeds.right,
+ (targetWheelSpeeds.right - m_prevSpeeds.right) / dt);
- auto leftOutput =
- volt_t(m_leftController->Calculate(
- m_leftSpeed().to<double>(), targetWheelSpeeds.left.to<double>())) +
- leftFeedforward;
+ auto leftOutput = volt_t(m_leftController->Calculate(
+ m_speeds().left.to<double>(),
+ targetWheelSpeeds.left.to<double>())) +
+ leftFeedforward;
auto rightOutput = volt_t(m_rightController->Calculate(
- m_rightSpeed().to<double>(),
+ m_speeds().right.to<double>(),
targetWheelSpeeds.right.to<double>())) +
rightFeedforward;
diff --git a/wpilibc/src/main/native/cpp/frc2/command/RunCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp
similarity index 100%
rename from wpilibc/src/main/native/cpp/frc2/command/RunCommand.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ScheduleCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
similarity index 100%
rename from wpilibc/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
diff --git a/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
similarity index 100%
rename from wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
diff --git a/wpilibc/src/main/native/cpp/frc2/command/StartEndCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp
similarity index 100%
rename from wpilibc/src/main/native/cpp/frc2/command/StartEndCommand.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp
diff --git a/wpilibc/src/main/native/cpp/frc2/command/Subsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
similarity index 100%
rename from wpilibc/src/main/native/cpp/frc2/command/Subsystem.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
diff --git a/wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
similarity index 96%
rename from wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
index 226f080..5e5ecbf 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
@@ -9,8 +9,9 @@
#include <frc/smartdashboard/SendableBuilder.h>
#include <frc/smartdashboard/SendableRegistry.h>
-#include <frc2/command/Command.h>
-#include <frc2/command/CommandScheduler.h>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandScheduler.h"
using namespace frc2;
diff --git a/wpilibc/src/main/native/cpp/frc2/command/WaitCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp
similarity index 100%
rename from wpilibc/src/main/native/cpp/frc2/command/WaitCommand.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp
diff --git a/wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
similarity index 83%
rename from wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
index d7a0daf..739a049 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
@@ -7,13 +7,15 @@
#include "frc2/command/WaitUntilCommand.h"
+#include "frc2/Timer.h"
+
using namespace frc2;
WaitUntilCommand::WaitUntilCommand(std::function<bool()> condition)
: m_condition{std::move(condition)} {}
-WaitUntilCommand::WaitUntilCommand(double time)
- : m_condition{[=] { return frc::Timer::GetMatchTime() - time > 0; }} {}
+WaitUntilCommand::WaitUntilCommand(units::second_t time)
+ : m_condition{[=] { return Timer::GetMatchTime() - time > 0_s; }} {}
bool WaitUntilCommand::IsFinished() { return m_condition(); }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
similarity index 72%
rename from wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
index e519a9f..440675f 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
@@ -16,8 +16,9 @@
return *this;
}
-Button Button::WhenPressed(std::function<void()> toRun) {
- WhenActive(std::move(toRun));
+Button Button::WhenPressed(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements) {
+ WhenActive(std::move(toRun), requirements);
return *this;
}
@@ -26,8 +27,9 @@
return *this;
}
-Button Button::WhileHeld(std::function<void()> toRun) {
- WhileActiveContinous(std::move(toRun));
+Button Button::WhileHeld(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements) {
+ WhileActiveContinous(std::move(toRun), requirements);
return *this;
}
@@ -41,8 +43,9 @@
return *this;
}
-Button Button::WhenReleased(std::function<void()> toRun) {
- WhenInactive(std::move(toRun));
+Button Button::WhenReleased(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements) {
+ WhenInactive(std::move(toRun), requirements);
return *this;
}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
similarity index 82%
rename from wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp
rename to wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
index 304bf98..caf188e 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
@@ -7,7 +7,7 @@
#include "frc2/command/button/Trigger.h"
-#include <frc2/command/InstantCommand.h>
+#include "frc2/command/InstantCommand.h"
using namespace frc2;
@@ -28,8 +28,9 @@
return *this;
}
-Trigger Trigger::WhenActive(std::function<void()> toRun) {
- return WhenActive(InstantCommand(std::move(toRun), {}));
+Trigger Trigger::WhenActive(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements) {
+ return WhenActive(InstantCommand(std::move(toRun), requirements));
}
Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) {
@@ -48,8 +49,10 @@
return *this;
}
-Trigger Trigger::WhileActiveContinous(std::function<void()> toRun) {
- return WhileActiveContinous(InstantCommand(std::move(toRun), {}));
+Trigger Trigger::WhileActiveContinous(
+ std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements) {
+ return WhileActiveContinous(InstantCommand(std::move(toRun), requirements));
}
Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) {
@@ -82,8 +85,9 @@
return *this;
}
-Trigger Trigger::WhenInactive(std::function<void()> toRun) {
- return WhenInactive(InstantCommand(std::move(toRun), {}));
+Trigger Trigger::WhenInactive(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements) {
+ return WhenInactive(InstantCommand(std::move(toRun), requirements));
}
Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) {
diff --git a/wpilibc/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
similarity index 94%
rename from wpilibc/src/main/native/include/frc2/command/Command.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/Command.h
index 49e904e..4695c26 100644
--- a/wpilibc/src/main/native/include/frc2/command/Command.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
@@ -7,18 +7,17 @@
#pragma once
-#include <frc/ErrorBase.h>
-#include <frc/WPIErrors.h>
-#include <frc2/command/Subsystem.h>
-
+#include <functional>
#include <memory>
#include <string>
+#include <frc/ErrorBase.h>
#include <units/units.h>
#include <wpi/ArrayRef.h>
#include <wpi/Demangle.h>
#include <wpi/SmallSet.h>
-#include <wpi/Twine.h>
+
+#include "frc2/command/Subsystem.h"
namespace frc2 {
@@ -128,17 +127,23 @@
* Decorates this command with a runnable to run before this command starts.
*
* @param toRun the Runnable to run
+ * @param requirements the required subsystems
* @return the decorated command
*/
- SequentialCommandGroup BeforeStarting(std::function<void()> toRun) &&;
+ SequentialCommandGroup BeforeStarting(
+ std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {}) &&;
/**
* Decorates this command with a runnable to run after the command finishes.
*
* @param toRun the Runnable to run
+ * @param requirements the required subsystems
* @return the decorated command
*/
- SequentialCommandGroup AndThen(std::function<void()> toRun) &&;
+ SequentialCommandGroup AndThen(
+ std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {}) &&;
/**
* Decorates this command to run perpetually, ignoring its ordinary end
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandBase.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h
similarity index 96%
rename from wpilibc/src/main/native/include/frc2/command/CommandBase.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h
index 3b1698f..81af2f7 100644
--- a/wpilibc/src/main/native/include/frc2/command/CommandBase.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h
@@ -7,15 +7,15 @@
#pragma once
-#include <frc/smartdashboard/Sendable.h>
-#include <frc/smartdashboard/SendableHelper.h>
-
+#include <initializer_list>
#include <string>
+#include <frc/smartdashboard/Sendable.h>
+#include <frc/smartdashboard/SendableHelper.h>
#include <wpi/SmallSet.h>
#include <wpi/Twine.h>
-#include "Command.h"
+#include "frc2/command/Command.h"
namespace frc2 {
/**
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h
similarity index 95%
rename from wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h
index 9d265b4..8b04b4c 100644
--- a/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h
@@ -7,13 +7,13 @@
#pragma once
-#include <frc/ErrorBase.h>
-
+#include <initializer_list>
#include <memory>
-#include <set>
#include <vector>
-#include "CommandBase.h"
+#include <wpi/ArrayRef.h>
+
+#include "frc2/command/CommandBase.h"
namespace frc2 {
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandHelper.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
similarity index 96%
rename from wpilibc/src/main/native/include/frc2/command/CommandHelper.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
index ec15f88..f78a848 100644
--- a/wpilibc/src/main/native/include/frc2/command/CommandHelper.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
@@ -11,7 +11,7 @@
#include <type_traits>
#include <utility>
-#include "Command.h"
+#include "frc2/command/Command.h"
namespace frc2 {
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
similarity index 86%
rename from wpilibc/src/main/native/include/frc2/command/CommandScheduler.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
index 2e9cdd5..c114d6a 100644
--- a/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
@@ -7,22 +7,16 @@
#pragma once
+#include <initializer_list>
+#include <memory>
+#include <utility>
+
#include <frc/ErrorBase.h>
-#include <frc/RobotState.h>
#include <frc/WPIErrors.h>
#include <frc/smartdashboard/Sendable.h>
#include <frc/smartdashboard/SendableHelper.h>
-
-#include <memory>
-#include <unordered_map>
-#include <utility>
-
-#include <networktables/NetworkTableEntry.h>
-#include <wpi/DenseMap.h>
+#include <wpi/ArrayRef.h>
#include <wpi/FunctionExtras.h>
-#include <wpi/SmallSet.h>
-
-#include "CommandState.h"
namespace frc2 {
class Command;
@@ -46,6 +40,10 @@
*/
static CommandScheduler& GetInstance();
+ ~CommandScheduler();
+ CommandScheduler(const CommandScheduler&) = delete;
+ CommandScheduler& operator=(const CommandScheduler&) = delete;
+
using Action = std::function<void(const Command&)>;
/**
@@ -186,8 +184,9 @@
"Default commands should not end!");
return;
}
- m_subsystems[subsystem] = std::make_unique<std::remove_reference_t<T>>(
- std::forward<T>(defaultCommand));
+ SetDefaultCommandImpl(subsystem,
+ std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(defaultCommand)));
}
/**
@@ -330,42 +329,11 @@
// Constructor; private as this is a singleton
CommandScheduler();
- // A map from commands to their scheduling state. Also used as a set of the
- // currently-running commands.
- wpi::DenseMap<Command*, CommandState> m_scheduledCommands;
+ void SetDefaultCommandImpl(Subsystem* subsystem,
+ std::unique_ptr<Command> command);
- // A map from required subsystems to their requiring commands. Also used as a
- // set of the currently-required subsystems.
- wpi::DenseMap<Subsystem*, Command*> m_requirements;
-
- // A map from subsystems registered with the scheduler to their default
- // commands. Also used as a list of currently-registered subsystems.
- wpi::DenseMap<Subsystem*, std::unique_ptr<Command>> m_subsystems;
-
- // The set of currently-registered buttons that will be polled every
- // iteration.
- wpi::SmallVector<wpi::unique_function<void()>, 4> m_buttons;
-
- bool m_disabled{false};
-
- // NetworkTable entries for use in Sendable impl
- nt::NetworkTableEntry m_namesEntry;
- nt::NetworkTableEntry m_idsEntry;
- nt::NetworkTableEntry m_cancelEntry;
-
- // Lists of user-supplied actions to be executed on scheduling events for
- // every command.
- wpi::SmallVector<Action, 4> m_initActions;
- wpi::SmallVector<Action, 4> m_executeActions;
- wpi::SmallVector<Action, 4> m_interruptActions;
- wpi::SmallVector<Action, 4> m_finishActions;
-
- // Flag and queues for avoiding concurrent modification if commands are
- // scheduled/canceled during run
-
- bool m_inRunLoop = false;
- wpi::DenseMap<Command*, bool> m_toSchedule;
- wpi::SmallVector<Command*, 4> m_toCancel;
+ class Impl;
+ std::unique_ptr<Impl> m_impl;
friend class CommandTestBase;
};
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandState.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandState.h
similarity index 100%
rename from wpilibc/src/main/native/include/frc2/command/CommandState.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/CommandState.h
diff --git a/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h
similarity index 95%
rename from wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h
index 0419cf9..af9f113 100644
--- a/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h
@@ -7,13 +7,13 @@
#pragma once
-#include <iostream>
+#include <functional>
#include <memory>
#include <utility>
-#include "CommandBase.h"
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
namespace frc2 {
/**
diff --git a/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h
similarity index 89%
rename from wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h
index b47135c..f85f9be 100644
--- a/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h
@@ -7,8 +7,10 @@
#pragma once
-#include "CommandBase.h"
-#include "CommandHelper.h"
+#include <functional>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
namespace frc2 {
/**
@@ -33,7 +35,8 @@
FunctionalCommand(std::function<void()> onInit,
std::function<void()> onExecute,
std::function<void(bool)> onEnd,
- std::function<bool()> isFinished);
+ std::function<bool()> isFinished,
+ std::initializer_list<Subsystem*> requirements = {});
FunctionalCommand(FunctionalCommand&& other) = default;
diff --git a/wpilibc/src/main/native/include/frc2/command/InstantCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h
similarity index 88%
rename from wpilibc/src/main/native/include/frc2/command/InstantCommand.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h
index ec28a11..97b4da2 100644
--- a/wpilibc/src/main/native/include/frc2/command/InstantCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h
@@ -7,8 +7,11 @@
#pragma once
-#include "CommandBase.h"
-#include "CommandHelper.h"
+#include <functional>
+#include <initializer_list>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
namespace frc2 {
/**
@@ -26,7 +29,7 @@
* @param requirements the subsystems required by this command
*/
InstantCommand(std::function<void()> toRun,
- std::initializer_list<Subsystem*> requirements);
+ std::initializer_list<Subsystem*> requirements = {});
InstantCommand(InstantCommand&& other) = default;
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h
new file mode 100644
index 0000000..e5a22a5
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h
@@ -0,0 +1,175 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+#include <functional>
+#include <memory>
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/kinematics/ChassisSpeeds.h>
+#include <frc/kinematics/MecanumDriveKinematics.h>
+#include <frc/kinematics/MecanumDriveWheelSpeeds.h>
+#include <frc/trajectory/Trajectory.h>
+#include <units/units.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "frc2/Timer.h"
+
+#pragma once
+
+namespace frc2 {
+/**
+ * A command that uses two PID controllers ({@link PIDController}) and a
+ * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
+ * {@link Trajectory} with a mecanum drive.
+ *
+ * <p>The command handles trajectory-following,
+ * Velocity PID calculations, and feedforwards internally. This
+ * is intended to be a more-or-less "complete solution" that can be used by
+ * teams without a great deal of controls expertise.
+ *
+ * <p>Advanced teams seeking more flexibility (for example, those who wish to
+ * use the onboard PID functionality of a "smart" motor controller) may use the
+ * secondary constructor that omits the PID and feedforward functionality,
+ * returning only the raw wheel speeds from the PID controllers.
+ *
+ * <p>The robot angle controller does not follow the angle given by
+ * the trajectory but rather goes to the angle given in the final state of the
+ * trajectory.
+ */
+class MecanumControllerCommand
+ : public CommandHelper<CommandBase, MecanumControllerCommand> {
+ public:
+ /**
+ * Constructs a new MecanumControllerCommand that when executed will follow
+ * the provided trajectory. PID control and feedforward are handled
+ * internally. Outputs are scaled from -12 to 12 as a voltage output to the
+ * motor.
+ *
+ * <p>Note: The controllers will *not* set the outputVolts to zero upon
+ * completion of the path this is left to the user, since it is not
+ * appropriate for paths with nonstationary endstates.
+ *
+ * <p>Note 2: The rotation controller will calculate the rotation based on the
+ * final pose in the trajectory, not the poses at each time step.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose,
+ * provided by the odometry class.
+ * @param feedforward The feedforward to use for the drivetrain.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param xController The Trajectory Tracker PID controller
+ * for the robot's x position.
+ * @param yController The Trajectory Tracker PID controller
+ * for the robot's y position.
+ * @param thetaController The Trajectory Tracker PID controller
+ * for angle for the robot.
+ * @param maxWheelVelocity The maximum velocity of a drivetrain wheel.
+ * @param frontLeftController The front left wheel velocity PID.
+ * @param rearLeftController The rear left wheel velocity PID.
+ * @param frontRightController The front right wheel velocity PID.
+ * @param rearRightController The rear right wheel velocity PID.
+ * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing
+ * the current wheel speeds.
+ * @param output The output of the velocity PIDs.
+ * @param requirements The subsystems to require.
+ */
+ MecanumControllerCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::SimpleMotorFeedforward<units::meters> feedforward,
+ frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+ frc2::PIDController yController,
+ frc::ProfiledPIDController<units::radians> thetaController,
+ units::meters_per_second_t maxWheelVelocity,
+ std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
+ frc2::PIDController frontLeftController,
+ frc2::PIDController rearLeftController,
+ frc2::PIDController frontRightController,
+ frc2::PIDController rearRightController,
+ std::function<void(units::volt_t, units::volt_t, units::volt_t,
+ units::volt_t)>
+ output,
+ std::initializer_list<Subsystem*> requirements);
+
+ /**
+ * Constructs a new MecanumControllerCommand that when executed will follow
+ * the provided trajectory. The user should implement a velocity PID on the
+ * desired output wheel velocities.
+ *
+ * <p>Note: The controllers will *not* set the outputVolts to zero upon
+ * completion of the path - this is left to the user, since it is not
+ * appropriate for paths with non-stationary end-states.
+ *
+ * <p>Note2: The rotation controller will calculate the rotation based on the
+ * final pose in the trajectory, not the poses at each time step.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose - use one
+ * of the odometry classes to provide this.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param xController The Trajectory Tracker PID controller
+ * for the robot's x position.
+ * @param yController The Trajectory Tracker PID controller
+ * for the robot's y position.
+ * @param thetaController The Trajectory Tracker PID controller
+ * for angle for the robot.
+ * @param maxWheelVelocity The maximum velocity of a drivetrain wheel.
+ * @param output The output of the position PIDs.
+ * @param requirements The subsystems to require.
+ */
+ MecanumControllerCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+ frc2::PIDController yController,
+ frc::ProfiledPIDController<units::radians> thetaController,
+ units::meters_per_second_t maxWheelVelocity,
+ std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+ units::meters_per_second_t,
+ units::meters_per_second_t)>
+ output,
+ std::initializer_list<Subsystem*> requirements);
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ private:
+ frc::Trajectory m_trajectory;
+ std::function<frc::Pose2d()> m_pose;
+ frc::SimpleMotorFeedforward<units::meters> m_feedforward;
+ frc::MecanumDriveKinematics m_kinematics;
+ std::unique_ptr<frc2::PIDController> m_xController;
+ std::unique_ptr<frc2::PIDController> m_yController;
+ std::unique_ptr<frc::ProfiledPIDController<units::radians>> m_thetaController;
+ const units::meters_per_second_t m_maxWheelVelocity;
+ std::unique_ptr<frc2::PIDController> m_frontLeftController;
+ std::unique_ptr<frc2::PIDController> m_rearLeftController;
+ std::unique_ptr<frc2::PIDController> m_frontRightController;
+ std::unique_ptr<frc2::PIDController> m_rearRightController;
+ std::function<frc::MecanumDriveWheelSpeeds()> m_currentWheelSpeeds;
+ std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+ units::meters_per_second_t, units::meters_per_second_t)>
+ m_outputVel;
+ std::function<void(units::volt_t, units::volt_t, units::volt_t,
+ units::volt_t)>
+ m_outputVolts;
+
+ bool m_usePID;
+ frc2::Timer m_timer;
+ frc::MecanumDriveWheelSpeeds m_prevSpeeds;
+ units::second_t m_prevTime;
+ frc::Pose2d m_finalPose;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
similarity index 92%
rename from wpilibc/src/main/native/include/frc2/command/NotifierCommand.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
index 3365c04..847c693 100644
--- a/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
@@ -7,12 +7,14 @@
#pragma once
-#include <frc/Notifier.h>
+#include <functional>
+#include <initializer_list>
+#include <frc/Notifier.h>
#include <units/units.h>
-#include "CommandBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
namespace frc2 {
/**
@@ -35,7 +37,7 @@
* @param requirements the subsystems required by this command
*/
NotifierCommand(std::function<void()> toRun, units::second_t period,
- std::initializer_list<Subsystem*> requirements);
+ std::initializer_list<Subsystem*> requirements = {});
NotifierCommand(NotifierCommand&& other);
diff --git a/wpilibc/src/main/native/include/frc2/command/PIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
similarity index 67%
rename from wpilibc/src/main/native/include/frc2/command/PIDCommand.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
index 6e553ff..1089fd1 100644
--- a/wpilibc/src/main/native/include/frc2/command/PIDCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
@@ -7,7 +7,11 @@
#pragma once
-#include "frc/controller/PIDController.h"
+#include <functional>
+#include <initializer_list>
+
+#include <frc/controller/PIDController.h>
+
#include "frc2/command/CommandBase.h"
#include "frc2/command/CommandHelper.h"
@@ -36,7 +40,7 @@
std::function<double()> measurementSource,
std::function<double()> setpointSource,
std::function<void(double)> useOutput,
- std::initializer_list<Subsystem*> requirements);
+ std::initializer_list<Subsystem*> requirements = {});
/**
* Creates a new PIDCommand, which controls the given output with a
@@ -64,56 +68,11 @@
void End(bool interrupted) override;
/**
- * Sets the function that uses the output of the PIDController.
- *
- * @param useOutput The function that uses the output.
- */
- void SetOutput(std::function<void(double)> useOutput);
-
- /**
- * Sets the setpoint for the controller to track the given source.
- *
- * @param setpointSource The setpoint source
- */
- void SetSetpoint(std::function<double()> setpointSource);
-
- /**
- * Sets the setpoint for the controller to a constant value.
- *
- * @param setpoint The setpoint
- */
- void SetSetpoint(double setpoint);
-
- /**
- * Sets the setpoint for the controller to a constant value relative (i.e.
- * added to) the current setpoint.
- *
- * @param relativeReference The change in setpoint
- */
- void SetSetpointRelative(double relativeSetpoint);
-
- /**
- * Gets the measurement of the process variable. Wraps the passed-in function
- * for readability.
- *
- * @return The measurement of the process variable
- */
- virtual double GetMeasurement();
-
- /**
- * Gets the measurement of the process variable. Wraps the passed-in function
- * for readability.
- *
- * @return The measurement of the process variable
- */
- virtual void UseOutput(double output);
-
- /**
* Returns the PIDController used by the command.
*
* @return The PIDController
*/
- PIDController& getController();
+ PIDController& GetController();
protected:
PIDController m_controller;
diff --git a/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
similarity index 77%
rename from wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
index 7aa21c0..62389c3 100644
--- a/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
@@ -7,7 +7,8 @@
#pragma once
-#include "frc/controller/PIDController.h"
+#include <frc/controller/PIDController.h>
+
#include "frc2/command/SubsystemBase.h"
namespace frc2 {
@@ -29,25 +30,11 @@
void Periodic() override;
/**
- * Uses the output from the PIDController.
+ * Sets the setpoint for the subsystem.
*
- * @param output the output of the PIDController
+ * @param setpoint the setpoint for the subsystem
*/
- virtual void UseOutput(double output) = 0;
-
- /**
- * Returns the reference (setpoint) used by the PIDController.
- *
- * @return the reference (setpoint) to be used by the controller
- */
- virtual double GetSetpoint() = 0;
-
- /**
- * Returns the measurement of the process variable used by the PIDController.
- *
- * @return the measurement of the process variable
- */
- virtual double GetMeasurement() = 0;
+ void SetSetpoint(double setpoint);
/**
* Enables the PID control. Resets the controller.
@@ -60,6 +47,13 @@
virtual void Disable();
/**
+ * Returns whether the controller is enabled.
+ *
+ * @return Whether the controller is enabled.
+ */
+ bool IsEnabled();
+
+ /**
* Returns the PIDController.
*
* @return The controller.
@@ -69,5 +63,23 @@
protected:
PIDController m_controller;
bool m_enabled;
+
+ /**
+ * Returns the measurement of the process variable used by the PIDController.
+ *
+ * @return the measurement of the process variable
+ */
+ virtual double GetMeasurement() = 0;
+
+ /**
+ * Uses the output from the PIDController.
+ *
+ * @param output the output of the PIDController
+ * @param setpoint the setpoint of the PIDController (for feedforward)
+ */
+ virtual void UseOutput(double output, double setpoint) = 0;
+
+ private:
+ double m_setpoint{0};
};
} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h
similarity index 95%
rename from wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h
index 322e7b1..5b0f0da 100644
--- a/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h
@@ -13,12 +13,11 @@
#endif
#include <memory>
-#include <unordered_map>
#include <utility>
#include <vector>
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
namespace frc2 {
/**
@@ -89,7 +88,7 @@
private:
void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
- std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
+ std::vector<std::pair<std::unique_ptr<Command>, bool>> m_commands;
bool m_runWhenDisabled{true};
bool isRunning = false;
};
diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
similarity index 95%
rename from wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
index 168d0f8..4debe75 100644
--- a/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
@@ -13,12 +13,11 @@
#endif
#include <memory>
-#include <unordered_map>
#include <utility>
#include <vector>
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
namespace frc2 {
/**
@@ -99,10 +98,10 @@
void SetDeadline(std::unique_ptr<Command>&& deadline);
- std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
+ std::vector<std::pair<std::unique_ptr<Command>, bool>> m_commands;
Command* m_deadline;
bool m_runWhenDisabled{true};
- bool isRunning = false;
+ bool m_finished{true};
};
} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h
similarity index 94%
rename from wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h
index d7411e9..167dee4 100644
--- a/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h
@@ -13,13 +13,11 @@
#endif
#include <memory>
-#include <set>
-#include <unordered_map>
#include <utility>
#include <vector>
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
namespace frc2 {
/**
@@ -78,7 +76,7 @@
private:
void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
- std::set<std::unique_ptr<Command>> m_commands;
+ std::vector<std::unique_ptr<Command>> m_commands;
bool m_runWhenDisabled{true};
bool m_finished{false};
bool isRunning = false;
diff --git a/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h
similarity index 95%
rename from wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h
index 3f3c9e7..4a93704 100644
--- a/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h
@@ -15,9 +15,9 @@
#include <memory>
#include <utility>
-#include "CommandBase.h"
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
namespace frc2 {
/**
diff --git a/wpilibc/src/main/native/include/frc2/command/PrintCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h
similarity index 92%
rename from wpilibc/src/main/native/include/frc2/command/PrintCommand.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h
index fb420cb..7b706a0 100644
--- a/wpilibc/src/main/native/include/frc2/command/PrintCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h
@@ -8,10 +8,9 @@
#pragma once
#include <wpi/Twine.h>
-#include <wpi/raw_ostream.h>
-#include "CommandHelper.h"
-#include "InstantCommand.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/InstantCommand.h"
namespace frc2 {
/**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
new file mode 100644
index 0000000..49575de
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
@@ -0,0 +1,145 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <initializer_list>
+#include <utility>
+
+#include <frc/controller/ProfiledPIDController.h>
+#include <units/units.h>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that controls an output with a ProfiledPIDController. Runs forever
+ * by default - to add exit conditions and/or other behavior, subclass this
+ * class. The controller calculation and output are performed synchronously in
+ * the command's execute() method.
+ *
+ * @see ProfiledPIDController<Distance>
+ */
+template <class Distance>
+class ProfiledPIDCommand
+ : public CommandHelper<CommandBase, ProfiledPIDCommand<Distance>> {
+ using Distance_t = units::unit_t<Distance>;
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Velocity_t = units::unit_t<Velocity>;
+ using State = typename frc::TrapezoidProfile<Distance>::State;
+
+ public:
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * ProfiledPIDController.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param goalSource the controller's goal
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+ std::function<Distance_t()> measurementSource,
+ std::function<State()> goalSource,
+ std::function<void(double, State)> useOutput,
+ std::initializer_list<Subsystem*> requirements = {})
+ : m_controller{controller},
+ m_measurement{std::move(measurementSource)},
+ m_goal{std::move(goalSource)},
+ m_useOutput{std::move(useOutput)} {
+ this->AddRequirements(requirements);
+ }
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * ProfiledPIDController.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param goalSource the controller's goal
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+ std::function<Distance_t()> measurementSource,
+ std::function<Distance_t()> goalSource,
+ std::function<void(double, State)> useOutput,
+ std::initializer_list<Subsystem*> requirements)
+ : ProfiledPIDCommand(controller, measurementSource,
+ [&goalSource]() {
+ return State{goalSource(), Velocity_t{0}};
+ },
+ useOutput, requirements) {}
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * ProfiledPIDController with a constant goal.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param goal the controller's goal
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+ std::function<Distance_t()> measurementSource, State goal,
+ std::function<void(double, State)> useOutput,
+ std::initializer_list<Subsystem*> requirements)
+ : ProfiledPIDCommand(controller, measurementSource,
+ [goal] { return goal; }, useOutput, requirements) {}
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * ProfiledPIDController with a constant goal.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param goal the controller's goal
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+ std::function<Distance_t()> measurementSource,
+ Distance_t goal,
+ std::function<void(double, State)> useOutput,
+ std::initializer_list<Subsystem*> requirements)
+ : ProfiledPIDCommand(controller, measurementSource,
+ [goal] { return goal; }, useOutput, requirements) {}
+
+ ProfiledPIDCommand(ProfiledPIDCommand&& other) = default;
+
+ ProfiledPIDCommand(const ProfiledPIDCommand& other) = default;
+
+ void Initialize() override { m_controller.Reset(); }
+
+ void Execute() override {
+ m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),
+ m_controller.GetSetpoint());
+ }
+
+ void End(bool interrupted) override {
+ m_useOutput(0, State{Distance_t(0), Velocity_t(0)});
+ }
+
+ /**
+ * Returns the ProfiledPIDController used by the command.
+ *
+ * @return The ProfiledPIDController
+ */
+ frc::ProfiledPIDController<Distance>& GetController() { return m_controller; }
+
+ protected:
+ frc::ProfiledPIDController<Distance> m_controller;
+ std::function<Distance_t()> m_measurement;
+ std::function<State()> m_goal;
+ std::function<void(double, State)> m_useOutput;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
new file mode 100644
index 0000000..d5302c5
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/controller/ProfiledPIDController.h>
+#include <units/units.h>
+
+#include "frc2/command/SubsystemBase.h"
+
+namespace frc2 {
+/**
+ * A subsystem that uses a ProfiledPIDController to control an output. The
+ * controller is run synchronously from the subsystem's periodic() method.
+ *
+ * @see ProfiledPIDController
+ */
+template <class Distance>
+class ProfiledPIDSubsystem : public SubsystemBase {
+ using Distance_t = units::unit_t<Distance>;
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Velocity_t = units::unit_t<Velocity>;
+ using State = typename frc::TrapezoidProfile<Distance>::State;
+
+ public:
+ /**
+ * Creates a new ProfiledPIDSubsystem.
+ *
+ * @param controller the ProfiledPIDController to use
+ */
+ explicit ProfiledPIDSubsystem(frc::ProfiledPIDController<Distance> controller)
+ : m_controller{controller} {}
+
+ void Periodic() override {
+ if (m_enabled) {
+ UseOutput(m_controller.Calculate(GetMeasurement(), m_goal),
+ m_controller.GetSetpoint());
+ }
+ }
+
+ /**
+ * Sets the goal state for the subsystem.
+ *
+ * @param goal The goal state for the subsystem's motion profile.
+ */
+ void SetGoal(State goal) { m_goal = goal; }
+
+ /**
+ * Sets the goal state for the subsystem. Goal velocity assumed to be zero.
+ *
+ * @param goal The goal position for the subsystem's motion profile.
+ */
+ void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
+
+ /**
+ * Enables the PID control. Resets the controller.
+ */
+ virtual void Enable() {
+ m_controller.Reset();
+ m_enabled = true;
+ }
+
+ /**
+ * Disables the PID control. Sets output to zero.
+ */
+ virtual void Disable() {
+ UseOutput(0, State{Distance_t(0), Velocity_t(0)});
+ m_enabled = false;
+ }
+
+ /**
+ * Returns whether the controller is enabled.
+ *
+ * @return Whether the controller is enabled.
+ */
+ bool IsEnabled() { return m_enabled; }
+
+ /**
+ * Returns the ProfiledPIDController.
+ *
+ * @return The controller.
+ */
+ frc::ProfiledPIDController<Distance>& GetController() { return m_controller; }
+
+ protected:
+ frc::ProfiledPIDController<Distance> m_controller;
+ bool m_enabled{false};
+
+ /**
+ * Returns the measurement of the process variable used by the
+ * ProfiledPIDController.
+ *
+ * @return the measurement of the process variable
+ */
+ virtual Distance_t GetMeasurement() = 0;
+
+ /**
+ * Uses the output from the ProfiledPIDController.
+ *
+ * @param output the output of the ProfiledPIDController
+ * @param setpoint the setpoint state of the ProfiledPIDController, for
+ * feedforward
+ */
+ virtual void UseOutput(double output, State setpoint) = 0;
+
+ private:
+ State m_goal;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h
similarity index 91%
rename from wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h
index 8906424..ef5204f 100644
--- a/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h
@@ -7,11 +7,12 @@
#pragma once
+#include <wpi/ArrayRef.h>
#include <wpi/SmallVector.h>
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "SetUtilities.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/SetUtilities.h"
namespace frc2 {
/**
diff --git a/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
similarity index 72%
rename from wpilibc/src/main/native/include/frc2/command/RamseteCommand.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
index 592f194..60d1b7e 100644
--- a/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
@@ -5,21 +5,23 @@
/* the project. */
/*----------------------------------------------------------------------------*/
+#pragma once
+
#include <functional>
+#include <initializer_list>
#include <memory>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/trajectory/Trajectory.h>
#include <units/units.h>
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "frc/controller/PIDController.h"
-#include "frc/controller/RamseteController.h"
-#include "frc/geometry/Pose2d.h"
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "frc/trajectory/Trajectory.h"
#include "frc2/Timer.h"
-
-#pragma once
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
namespace frc2 {
/**
@@ -40,18 +42,11 @@
* @see Trajectory
*/
class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
- using voltsecondspermeter =
- units::compound_unit<units::volt, units::second,
- units::inverse<units::meter>>;
- using voltsecondssquaredpermeter =
- units::compound_unit<units::volt, units::squared<units::second>,
- units::inverse<units::meter>>;
-
public:
/**
* Constructs a new RamseteCommand that, when executed, will follow the
* provided trajectory. PID control and feedforward are handled internally,
- * and outputs are scaled -1 to 1 for easy consumption by speed controllers.
+ * and outputs are scaled -12 to 12 representing units of volts.
*
* <p>Note: The controller will *not* set the outputVolts to zero upon
* completion of the path - this is left to the user, since it is not
@@ -62,16 +57,11 @@
* the odometry classes to provide this.
* @param controller The RAMSETE controller used to follow the
* trajectory.
- * @param ks Constant feedforward term for the robot drive.
- * @param kv Velocity-proportional feedforward term for the robot
+ * @param feedforward A component for calculating the feedforward for the
* drive.
- * @param ka Acceleration-proportional feedforward term for the
- * robot drive.
* @param kinematics The kinematics for the robot drivetrain.
- * @param leftSpeed A function that supplies the speed of the left side
- * of the robot drive.
- * @param rightSpeed A function that supplies the speed of the right side
- * of the robot drive.
+ * @param wheelSpeeds A function that supplies the speeds of the left
+ * and right sides of the robot drive.
* @param leftController The PIDController for the left side of the robot
* drive.
* @param rightController The PIDController for the right side of the robot
@@ -81,16 +71,14 @@
* @param requirements The subsystems to require.
*/
RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
- frc::RamseteController controller, units::volt_t ks,
- units::unit_t<voltsecondspermeter> kv,
- units::unit_t<voltsecondssquaredpermeter> ka,
+ frc::RamseteController controller,
+ frc::SimpleMotorFeedforward<units::meters> feedforward,
frc::DifferentialDriveKinematics kinematics,
- std::function<units::meters_per_second_t()> leftSpeed,
- std::function<units::meters_per_second_t()> rightSpeed,
+ std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
frc2::PIDController leftController,
frc2::PIDController rightController,
std::function<void(units::volt_t, units::volt_t)> output,
- std::initializer_list<Subsystem*> requirements);
+ std::initializer_list<Subsystem*> requirements = {});
/**
* Constructs a new RamseteCommand that, when executed, will follow the
@@ -105,7 +93,7 @@
* trajectory.
* @param kinematics The kinematics for the robot drivetrain.
* @param output A function that consumes the computed left and right
- * outputs (in volts) for the robot drive.
+ * wheel speeds.
* @param requirements The subsystems to require.
*/
RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
@@ -128,12 +116,9 @@
frc::Trajectory m_trajectory;
std::function<frc::Pose2d()> m_pose;
frc::RamseteController m_controller;
- const units::volt_t m_ks;
- const units::unit_t<voltsecondspermeter> m_kv;
- const units::unit_t<voltsecondssquaredpermeter> m_ka;
+ frc::SimpleMotorFeedforward<units::meters> m_feedforward;
frc::DifferentialDriveKinematics m_kinematics;
- std::function<units::meters_per_second_t()> m_leftSpeed;
- std::function<units::meters_per_second_t()> m_rightSpeed;
+ std::function<frc::DifferentialDriveWheelSpeeds()> m_speeds;
std::unique_ptr<frc2::PIDController> m_leftController;
std::unique_ptr<frc2::PIDController> m_rightController;
std::function<void(units::volt_t, units::volt_t)> m_outputVolts;
@@ -143,5 +128,6 @@
Timer m_timer;
units::second_t m_prevTime;
frc::DifferentialDriveWheelSpeeds m_prevSpeeds;
+ bool m_usePID;
};
} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/RunCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h
similarity index 86%
rename from wpilibc/src/main/native/include/frc2/command/RunCommand.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h
index 5a0524a..aae7b74 100644
--- a/wpilibc/src/main/native/include/frc2/command/RunCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h
@@ -7,8 +7,11 @@
#pragma once
-#include "CommandBase.h"
-#include "CommandHelper.h"
+#include <functional>
+#include <initializer_list>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
namespace frc2 {
/**
@@ -27,7 +30,7 @@
* @param requirements the subsystems to require
*/
RunCommand(std::function<void()> toRun,
- std::initializer_list<Subsystem*> requirements);
+ std::initializer_list<Subsystem*> requirements = {});
RunCommand(RunCommand&& other) = default;
diff --git a/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h
similarity index 90%
rename from wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h
index 422823b..c2824ca 100644
--- a/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h
@@ -7,11 +7,12 @@
#pragma once
+#include <wpi/ArrayRef.h>
#include <wpi/SmallVector.h>
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "SetUtilities.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/SetUtilities.h"
namespace frc2 {
/**
diff --git a/wpilibc/src/main/native/include/frc2/command/SelectCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
similarity index 96%
rename from wpilibc/src/main/native/include/frc2/command/SelectCommand.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
index 8dba378..e1074f4 100644
--- a/wpilibc/src/main/native/include/frc2/command/SelectCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
@@ -13,16 +13,16 @@
#endif
#include <memory>
+#include <type_traits>
#include <unordered_map>
#include <utility>
#include <vector>
-#include "CommandBase.h"
-#include "CommandGroupBase.h"
-#include "PrintCommand.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/PrintCommand.h"
namespace frc2 {
-template <typename Key>
/**
* Runs one of a selection of commands, either using a selector and a key to
* command mapping, or a supplier that returns the command directly at runtime.
@@ -39,6 +39,7 @@
* <p>As a rule, CommandGroups require the union of the requirements of their
* component commands.
*/
+template <typename Key>
class SelectCommand : public CommandHelper<CommandBase, SelectCommand<Key>> {
public:
/**
diff --git a/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h
similarity index 94%
rename from wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h
index dd1f2ce..c49356e 100644
--- a/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h
@@ -14,15 +14,16 @@
#include <limits>
#include <memory>
+#include <type_traits>
#include <utility>
#include <vector>
+#include <frc/ErrorBase.h>
+#include <frc/WPIErrors.h>
#include <wpi/ArrayRef.h>
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
-#include "frc/ErrorBase.h"
-#include "frc/WPIErrors.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
namespace frc2 {
diff --git a/wpilibc/src/main/native/include/frc2/command/SetUtilities.h b/wpilibNewCommands/src/main/native/include/frc2/command/SetUtilities.h
similarity index 100%
rename from wpilibc/src/main/native/include/frc2/command/SetUtilities.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/SetUtilities.h
diff --git a/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
similarity index 92%
rename from wpilibc/src/main/native/include/frc2/command/StartEndCommand.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
index 987b9db..32b7e76 100644
--- a/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
@@ -7,8 +7,11 @@
#pragma once
-#include "CommandBase.h"
-#include "CommandHelper.h"
+#include <functional>
+#include <initializer_list>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
namespace frc2 {
/**
@@ -29,7 +32,7 @@
* @param requirements the subsystems required by this command
*/
StartEndCommand(std::function<void()> onInit, std::function<void()> onEnd,
- std::initializer_list<Subsystem*> requirements);
+ std::initializer_list<Subsystem*> requirements = {});
StartEndCommand(StartEndCommand&& other) = default;
diff --git a/wpilibc/src/main/native/include/frc2/command/Subsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
similarity index 97%
rename from wpilibc/src/main/native/include/frc2/command/Subsystem.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
index 5eb10bc..687510d 100644
--- a/wpilibc/src/main/native/include/frc2/command/Subsystem.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
@@ -7,10 +7,11 @@
#pragma once
-#include <frc2/command/CommandScheduler.h>
-
+#include <type_traits>
#include <utility>
+#include "frc2/command/CommandScheduler.h"
+
namespace frc2 {
class Command;
/**
diff --git a/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h b/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
similarity index 97%
rename from wpilibc/src/main/native/include/frc2/command/SubsystemBase.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
index e7dffc7..d75ad0d 100644
--- a/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
@@ -7,12 +7,12 @@
#pragma once
+#include <string>
+
#include <frc/smartdashboard/Sendable.h>
#include <frc/smartdashboard/SendableHelper.h>
-#include <string>
-
-#include "Subsystem.h"
+#include "frc2/command/Subsystem.h"
namespace frc2 {
/**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
new file mode 100644
index 0000000..ab3fe10
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+#include <functional>
+#include <memory>
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/kinematics/ChassisSpeeds.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <frc/trajectory/Trajectory.h>
+#include <units/units.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "frc2/Timer.h"
+
+#pragma once
+
+namespace frc2 {
+
+/**
+ * A command that uses two PID controllers ({@link PIDController}) and a
+ * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
+ * {@link Trajectory} with a swerve drive.
+ *
+ * <p>The command handles trajectory-following, Velocity PID calculations, and
+ * feedforwards internally. This is intended to be a more-or-less "complete
+ * solution" that can be used by teams without a great deal of controls
+ * expertise.
+ *
+ * <p>Advanced teams seeking more flexibility (for example, those who wish to
+ * use the onboard PID functionality of a "smart" motor controller) may use the
+ * secondary constructor that omits the PID and feedforward functionality,
+ * returning only the raw module states from the position PID controllers.
+ *
+ * <p>The robot angle controller does not follow the angle given by
+ * the trajectory but rather goes to the angle given in the final state of the
+ * trajectory.
+ */
+template <size_t NumModules>
+class SwerveControllerCommand
+ : public CommandHelper<CommandBase, SwerveControllerCommand<NumModules>> {
+ using voltsecondspermeter =
+ units::compound_unit<units::voltage::volt, units::second,
+ units::inverse<units::meter>>;
+ using voltsecondssquaredpermeter =
+ units::compound_unit<units::voltage::volt, units::squared<units::second>,
+ units::inverse<units::meter>>;
+
+ public:
+ /**
+ * Constructs a new SwerveControllerCommand that when executed will follow the
+ * provided trajectory. This command will not return output voltages but
+ * rather raw module states from the position controllers which need to be put
+ * into a velocity PID.
+ *
+ * <p>Note: The controllers will *not* set the outputVolts to zero upon
+ * completion of the path- this is left to the user, since it is not
+ * appropriate for paths with nonstationary endstates.
+ *
+ * <p>Note 2: The rotation controller will calculate the rotation based on the
+ * final pose in the trajectory, not the poses at each time step.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose,
+ * provided by the odometry class.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param xController The Trajectory Tracker PID controller
+ * for the robot's x position.
+ * @param yController The Trajectory Tracker PID controller
+ * for the robot's y position.
+ * @param thetaController The Trajectory Tracker PID controller
+ * for angle for the robot.
+ * @param output The raw output module states from the
+ * position controllers.
+ * @param requirements The subsystems to require.
+ */
+ SwerveControllerCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::SwerveDriveKinematics<NumModules> kinematics,
+ frc2::PIDController xController, frc2::PIDController yController,
+ frc::ProfiledPIDController<units::radians> thetaController,
+ std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
+ output,
+ std::initializer_list<Subsystem*> requirements);
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ private:
+ frc::Trajectory m_trajectory;
+ std::function<frc::Pose2d()> m_pose;
+ frc::SwerveDriveKinematics<NumModules> m_kinematics;
+ std::unique_ptr<frc2::PIDController> m_xController;
+ std::unique_ptr<frc2::PIDController> m_yController;
+ std::unique_ptr<frc::ProfiledPIDController<units::radians>> m_thetaController;
+ std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
+ m_outputStates;
+
+ frc2::Timer m_timer;
+ units::second_t m_prevTime;
+ frc::Pose2d m_finalPose;
+};
+} // namespace frc2
+
+#include "SwerveControllerCommand.inc"
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
new file mode 100644
index 0000000..7beafef
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+namespace frc2 {
+
+template <size_t NumModules>
+SwerveControllerCommand<NumModules>::SwerveControllerCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::SwerveDriveKinematics<NumModules> kinematics,
+ frc2::PIDController xController, frc2::PIDController yController,
+ frc::ProfiledPIDController<units::radians> thetaController,
+ std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
+ std::initializer_list<Subsystem*> requirements)
+ : m_trajectory(trajectory),
+ m_pose(pose),
+ m_kinematics(kinematics),
+ m_xController(std::make_unique<frc2::PIDController>(xController)),
+ m_yController(std::make_unique<frc2::PIDController>(yController)),
+ m_thetaController(
+ std::make_unique<frc::ProfiledPIDController<units::radians>>(
+ thetaController)),
+ m_outputStates(output) {
+ this->AddRequirements(requirements);
+}
+
+template <size_t NumModules>
+void SwerveControllerCommand<NumModules>::Initialize() {
+ m_finalPose = m_trajectory.Sample(m_trajectory.TotalTime()).pose;
+
+ m_timer.Reset();
+ m_timer.Start();
+}
+
+template <size_t NumModules>
+void SwerveControllerCommand<NumModules>::Execute() {
+ auto curTime = units::second_t(m_timer.Get());
+
+ auto m_desiredState = m_trajectory.Sample(curTime);
+ auto m_desiredPose = m_desiredState.pose;
+
+ auto m_poseError = m_desiredPose.RelativeTo(m_pose());
+
+ auto targetXVel = units::meters_per_second_t(m_xController->Calculate(
+ (m_pose().Translation().X().template to<double>()),
+ (m_desiredPose.Translation().X().template to<double>())));
+ auto targetYVel = units::meters_per_second_t(m_yController->Calculate(
+ (m_pose().Translation().Y().template to<double>()),
+ (m_desiredPose.Translation().Y().template to<double>())));
+
+ // Profiled PID Controller only takes meters as setpoint and measurement
+ // The robot will go to the desired rotation of the final pose in the
+ // trajectory, not following the poses at individual states.
+ auto targetAngularVel =
+ units::radians_per_second_t(m_thetaController->Calculate(
+ m_pose().Rotation().Radians(), m_finalPose.Rotation().Radians()));
+
+ auto vRef = m_desiredState.velocity;
+
+ targetXVel += vRef * m_poseError.Rotation().Cos();
+ targetYVel += vRef * m_poseError.Rotation().Sin();
+
+ auto targetChassisSpeeds =
+ frc::ChassisSpeeds{targetXVel, targetYVel, targetAngularVel};
+
+ auto targetModuleStates =
+ m_kinematics.ToSwerveModuleStates(targetChassisSpeeds);
+
+ m_outputStates(targetModuleStates);
+}
+
+template <size_t NumModules>
+void SwerveControllerCommand<NumModules>::End(bool interrupted) {
+ m_timer.Stop();
+}
+
+template <size_t NumModules>
+bool SwerveControllerCommand<NumModules>::IsFinished() {
+ return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
+}
+
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
new file mode 100644
index 0000000..e21de07
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <initializer_list>
+
+#include <frc/trajectory/TrapezoidProfile.h>
+
+#include "frc2/Timer.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that runs a TrapezoidProfile. Useful for smoothly controlling
+ * mechanism motion.
+ *
+ * @see TrapezoidProfile
+ */
+template <class Distance>
+class TrapezoidProfileCommand
+ : public CommandHelper<CommandBase, TrapezoidProfileCommand<Distance>> {
+ using Distance_t = units::unit_t<Distance>;
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Velocity_t = units::unit_t<Velocity>;
+ using State = typename frc::TrapezoidProfile<Distance>::State;
+
+ public:
+ /**
+ * Creates a new TrapezoidProfileCommand that will execute the given
+ * TrapezoidalProfile. Output will be piped to the provided consumer function.
+ *
+ * @param profile The motion profile to execute.
+ * @param output The consumer for the profile output.
+ */
+ TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
+ std::function<void(State)> output,
+ std::initializer_list<Subsystem*> requirements)
+ : m_profile(profile), m_output(output) {
+ this->AddRequirements(requirements);
+ }
+
+ void Initialize() override {
+ m_timer.Reset();
+ m_timer.Start();
+ }
+
+ void Execute() override { m_output(m_profile.Calculate(m_timer.Get())); }
+
+ void End(bool interrupted) override { m_timer.Stop(); }
+
+ bool IsFinished() override {
+ return m_timer.HasPeriodPassed(m_profile.TotalTime());
+ }
+
+ private:
+ frc::TrapezoidProfile<Distance> m_profile;
+ std::function<void(State)> m_output;
+
+ Timer m_timer;
+};
+
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
new file mode 100644
index 0000000..a833f3c
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <units/units.h>
+
+#include "frc2/command/SubsystemBase.h"
+
+namespace frc2 {
+/**
+ * A subsystem that generates and runs trapezoidal motion profiles
+ * automatically. The user specifies how to use the current state of the motion
+ * profile by overriding the `UseState` method.
+ */
+template <class Distance>
+class TrapezoidProfileSubsystem : public SubsystemBase {
+ using Distance_t = units::unit_t<Distance>;
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Velocity_t = units::unit_t<Velocity>;
+ using State = typename frc::TrapezoidProfile<Distance>::State;
+ using Constraints = typename frc::TrapezoidProfile<Distance>::Constraints;
+
+ public:
+ /**
+ * Creates a new TrapezoidProfileSubsystem.
+ *
+ * @param constraints The constraints (maximum velocity and acceleration)
+ * for the profiles.
+ * @param initialPosition The initial position of the controller mechanism
+ * when the subsystem is constructed.
+ * @param period The period of the main robot loop, in seconds.
+ */
+ TrapezoidProfileSubsystem(Constraints constraints, Distance_t position,
+ units::second_t period = 20_ms)
+ : m_constraints(constraints),
+ m_state{position, Velocity_t(0)},
+ m_goal{position, Velocity_t{0}},
+ m_period(period) {}
+
+ void Periodic() override {
+ auto profile =
+ frc::TrapezoidProfile<Distance>(m_constraints, m_goal, m_state);
+ m_state = profile.Calculate(m_period);
+ UseState(m_state);
+ }
+
+ /**
+ * Sets the goal state for the subsystem.
+ *
+ * @param goal The goal state for the subsystem's motion profile.
+ */
+ void SetGoal(State goal) { m_goal = goal; }
+
+ /**
+ * Sets the goal state for the subsystem. Goal velocity assumed to be zero.
+ *
+ * @param goal The goal position for the subsystem's motion profile.
+ */
+ void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
+
+ protected:
+ /**
+ * Users should override this to consume the current state of the motion
+ * profile.
+ *
+ * @param state The current state of the motion profile.
+ */
+ virtual void UseState(State state) = 0;
+
+ private:
+ Constraints m_constraints;
+ State m_state;
+ State m_goal;
+ units::second_t m_period;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/WaitCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
similarity index 94%
rename from wpilibc/src/main/native/include/frc2/command/WaitCommand.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
index 1544592..ab97958 100644
--- a/wpilibc/src/main/native/include/frc2/command/WaitCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
@@ -8,11 +8,10 @@
#pragma once
#include <units/units.h>
-#include <wpi/Twine.h>
-#include "CommandBase.h"
-#include "CommandHelper.h"
#include "frc2/Timer.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
namespace frc2 {
/**
diff --git a/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h
similarity index 92%
rename from wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h
index 8da18e8..50b4855 100644
--- a/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h
@@ -7,8 +7,11 @@
#pragma once
-#include "CommandBase.h"
-#include "frc/Timer.h"
+#include <functional>
+
+#include <units/units.h>
+
+#include "frc2/command/CommandBase.h"
#include "frc2/command/CommandHelper.h"
namespace frc2 {
@@ -36,7 +39,7 @@
*
* @param time the match time after which to end, in seconds
*/
- explicit WaitUntilCommand(double time);
+ explicit WaitUntilCommand(units::second_t time);
WaitUntilCommand(WaitUntilCommand&& other) = default;
diff --git a/wpilibc/src/main/native/include/frc2/command/button/Button.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
similarity index 93%
rename from wpilibc/src/main/native/include/frc2/command/button/Button.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
index 3e0f7fe..6561652 100644
--- a/wpilibc/src/main/native/include/frc2/command/button/Button.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
@@ -65,8 +65,10 @@
* Binds a runnable to execute when the button is pressed.
*
* @param toRun the runnable to execute.
+ * @param requirements the required subsystems.
*/
- Button WhenPressed(std::function<void()> toRun);
+ Button WhenPressed(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {});
/**
* Binds a command to be started repeatedly while the button is pressed, and
@@ -100,8 +102,10 @@
* Binds a runnable to execute repeatedly while the button is pressed.
*
* @param toRun the runnable to execute.
+ * @param requirements the required subsystems.
*/
- Button WhileHeld(std::function<void()> toRun);
+ Button WhileHeld(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {});
/**
* Binds a command to be started when the button is pressed, and cancelled
@@ -163,8 +167,10 @@
* Binds a runnable to execute when the button is released.
*
* @param toRun the runnable to execute.
+ * @param requirements the required subsystems.
*/
- Button WhenReleased(std::function<void()> toRun);
+ Button WhenReleased(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {});
/**
* Binds a command to start when the button is pressed, and be cancelled when
diff --git a/wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/JoystickButton.h
similarity index 100%
rename from wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/button/JoystickButton.h
diff --git a/wpilibc/src/main/native/include/frc2/command/button/POVButton.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.h
similarity index 100%
rename from wpilibc/src/main/native/include/frc2/command/button/POVButton.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.h
diff --git a/wpilibc/src/main/native/include/frc2/command/button/Trigger.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
similarity index 94%
rename from wpilibc/src/main/native/include/frc2/command/button/Trigger.h
rename to wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
index 1a65ff6..efc4bc0 100644
--- a/wpilibc/src/main/native/include/frc2/command/button/Trigger.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
@@ -7,13 +7,13 @@
#pragma once
-#include <frc2/command/Command.h>
-#include <frc2/command/CommandScheduler.h>
-
#include <atomic>
#include <memory>
#include <utility>
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandScheduler.h"
+
namespace frc2 {
class Command;
/**
@@ -96,8 +96,10 @@
* Binds a runnable to execute when the trigger becomes active.
*
* @param toRun the runnable to execute.
+ * @paaram requirements the required subsystems.
*/
- Trigger WhenActive(std::function<void()> toRun);
+ Trigger WhenActive(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {});
/**
* Binds a command to be started repeatedly while the trigger is active, and
@@ -145,8 +147,11 @@
* Binds a runnable to execute repeatedly while the trigger is active.
*
* @param toRun the runnable to execute.
+ * @param requirements the required subsystems.
*/
- Trigger WhileActiveContinous(std::function<void()> toRun);
+ Trigger WhileActiveContinous(
+ std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {});
/**
* Binds a command to be started when the trigger becomes active, and
@@ -234,8 +239,10 @@
* Binds a runnable to execute when the trigger becomes inactive.
*
* @param toRun the runnable to execute.
+ * @param requirements the required subsystems.
*/
- Trigger WhenInactive(std::function<void()> toRun);
+ Trigger WhenInactive(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {});
/**
* Binds a command to start when the trigger becomes active, and be cancelled
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java
new file mode 100644
index 0000000..4f9abf0
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/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.wpilibj2;
+
+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/wpilibj2/command/ButtonTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
new file mode 100644
index 0000000..2ac2129
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* 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.wpilibj2.command;
+
+import java.util.ArrayList;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+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.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class MecanumControllerCommandTest {
+ private final Timer m_timer = new Timer();
+ private Rotation2d m_angle = new Rotation2d(0);
+
+ private double m_frontLeftSpeed;
+ private double m_rearLeftSpeed;
+ private double m_frontRightSpeed;
+ private double m_rearRightSpeed;
+
+ private final ProfiledPIDController m_rotController = new ProfiledPIDController(1, 0, 0,
+ new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
+
+ private static final double kxTolerance = 1 / 12.0;
+ private static final double kyTolerance = 1 / 12.0;
+ private static final double kAngularTolerance = 1 / 12.0;
+
+ private static final double kWheelBase = 0.5;
+ private static final double kTrackWidth = 0.5;
+
+ private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(
+ new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+
+ private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
+ new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0)));
+
+ public void setWheelSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
+ this.m_frontLeftSpeed = wheelSpeeds.frontLeftMetersPerSecond;
+ this.m_rearLeftSpeed = wheelSpeeds.rearLeftMetersPerSecond;
+ this.m_frontRightSpeed = wheelSpeeds.frontRightMetersPerSecond;
+ this.m_rearRightSpeed = wheelSpeeds.rearRightMetersPerSecond;
+ }
+
+ public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
+ return new MecanumDriveWheelSpeeds(m_frontLeftSpeed,
+ m_frontRightSpeed, m_rearLeftSpeed, m_rearRightSpeed);
+ }
+
+ public Pose2d getRobotPose() {
+ m_odometry.updateWithTime(m_timer.get(), m_angle, getCurrentWheelSpeeds());
+ return m_odometry.getPoseMeters();
+ }
+
+ @Test
+ @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+ void testReachesReference() {
+ final var subsystem = new Subsystem() {};
+
+ final var waypoints = new ArrayList<Pose2d>();
+ waypoints.add(new Pose2d(0, 0, new Rotation2d(0)));
+ waypoints.add(new Pose2d(1, 5, new Rotation2d(3)));
+ var config = new TrajectoryConfig(8.8, 0.1);
+ final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+ final var endState = trajectory.sample(trajectory.getTotalTimeSeconds());
+
+ final var command = new MecanumControllerCommand(trajectory,
+ this::getRobotPose,
+ m_kinematics,
+ new PIDController(0.6, 0, 0),
+ new PIDController(0.6, 0, 0),
+ m_rotController,
+ 8.8,
+ this::setWheelSpeeds,
+ subsystem);
+
+ m_timer.reset();
+ m_timer.start();
+
+ command.initialize();
+ while (!command.isFinished()) {
+ command.execute();
+ m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation();
+ }
+ m_timer.stop();
+ command.end(true);
+
+ assertAll(
+ () -> assertEquals(endState.poseMeters.getTranslation().getX(),
+ getRobotPose().getTranslation().getX(), kxTolerance),
+ () -> assertEquals(endState.poseMeters.getTranslation().getY(),
+ getRobotPose().getTranslation().getY(), kyTolerance),
+ () -> assertEquals(endState.poseMeters.getRotation().getRadians(),
+ getRobotPose().getRotation().getRadians(), kAngularTolerance)
+ );
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java
similarity index 80%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java
index 1bed736..fb06844 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java
@@ -13,7 +13,7 @@
import edu.wpi.first.wpilibj.Timer;
-import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
@DisabledOnOs(OS.MAC)
class NotifierCommandTest extends CommandTestBase {
@@ -23,12 +23,13 @@
Counter counter = new Counter();
- NotifierCommand command = new NotifierCommand(counter::increment, .01);
+ NotifierCommand command = new NotifierCommand(counter::increment, 0.01);
scheduler.schedule(command);
- Timer.delay(.25);
+ Timer.delay(0.25);
scheduler.cancel(command);
- assertEquals(.25, 0.01 * counter.m_counter, .025);
+ assertTrue(counter.m_counter > 10, "Should have hit at least 10 triggers");
+ assertTrue(counter.m_counter < 100, "Shouldn't hit more then 100 triggers");
}
}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java
new file mode 100644
index 0000000..5209199
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* 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.wpilibj2.command;
+
+import java.util.ArrayList;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+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.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class SwerveControllerCommandTest {
+ private final Timer m_timer = new Timer();
+ private Rotation2d m_angle = new Rotation2d(0);
+
+ private SwerveModuleState[] m_moduleStates = new SwerveModuleState[]{
+ new SwerveModuleState(0, new Rotation2d(0)),
+ new SwerveModuleState(0, new Rotation2d(0)),
+ new SwerveModuleState(0, new Rotation2d(0)),
+ new SwerveModuleState(0, new Rotation2d(0))};
+
+ private final ProfiledPIDController m_rotController = new ProfiledPIDController(1, 0, 0,
+ new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
+
+ private static final double kxTolerance = 1 / 12.0;
+ private static final double kyTolerance = 1 / 12.0;
+ private static final double kAngularTolerance = 1 / 12.0;
+
+ private static final double kWheelBase = 0.5;
+ private static final double kTrackWidth = 0.5;
+
+ private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
+ new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+
+ private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
+ new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0)));
+
+ @SuppressWarnings("PMD.ArrayIsStoredDirectly")
+ public void setModuleStates(SwerveModuleState[] moduleStates) {
+ this.m_moduleStates = moduleStates;
+ }
+
+ public Pose2d getRobotPose() {
+ m_odometry.updateWithTime(m_timer.get(), m_angle, m_moduleStates);
+ return m_odometry.getPoseMeters();
+ }
+
+ @Test
+ @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+ void testReachesReference() {
+ final var subsystem = new Subsystem() {};
+
+ final var waypoints = new ArrayList<Pose2d>();
+ waypoints.add(new Pose2d(0, 0, new Rotation2d(0)));
+ waypoints.add(new Pose2d(1, 5, new Rotation2d(3)));
+ var config = new TrajectoryConfig(8.8, 0.1);
+ final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+ final var endState = trajectory.sample(trajectory.getTotalTimeSeconds());
+
+ final var command = new SwerveControllerCommand(trajectory,
+ this::getRobotPose,
+ m_kinematics,
+ new PIDController(0.6, 0, 0),
+ new PIDController(0.6, 0, 0),
+ m_rotController,
+ this::setModuleStates,
+ subsystem);
+
+ m_timer.reset();
+ m_timer.start();
+
+ command.initialize();
+ while (!command.isFinished()) {
+ command.execute();
+ m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation();
+ }
+ m_timer.stop();
+ command.end(true);
+
+ assertAll(
+ () -> assertEquals(endState.poseMeters.getTranslation().getX(),
+ getRobotPose().getTranslation().getX(), kxTolerance),
+ () -> assertEquals(endState.poseMeters.getTranslation().getY(),
+ getRobotPose().getTranslation().getY(), kyTolerance),
+ () -> assertEquals(endState.poseMeters.getRotation().getRadians(),
+ getRobotPose().getRotation().getRadians(), kAngularTolerance)
+ );
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java
rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
similarity index 97%
rename from wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
index 0fe0b5b..5e876c4 100644
--- a/wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
@@ -18,7 +18,7 @@
TEST_F(CommandDecoratorTest, WithTimeoutTest) {
CommandScheduler scheduler = GetScheduler();
- auto command = RunCommand([] {}, {}).WithTimeout(.1_s);
+ auto command = RunCommand([] {}, {}).WithTimeout(100_ms);
scheduler.Schedule(&command);
diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.h
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.h
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.h
diff --git a/wpilibc/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/InstantCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/InstantCommandTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/InstantCommandTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/InstantCommandTest.cpp
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp
new file mode 100644
index 0000000..4eae880
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc2/Timer.h>
+#include <frc2/command/MecanumControllerCommand.h>
+#include <frc2/command/Subsystem.h>
+
+#include <iostream>
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/MecanumDriveKinematics.h>
+#include <frc/kinematics/MecanumDriveOdometry.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <wpi/math>
+
+#include "gtest/gtest.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+ EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+class MecanumControllerCommandTest : public ::testing::Test {
+ using radians_per_second_squared_t =
+ units::compound_unit<units::radians,
+ units::inverse<units::squared<units::second>>>;
+
+ protected:
+ frc2::Timer m_timer;
+ frc::Rotation2d m_angle{0_rad};
+
+ units::meters_per_second_t m_frontLeftSpeed = 0.0_mps;
+ units::meters_per_second_t m_rearLeftSpeed = 0.0_mps;
+ units::meters_per_second_t m_frontRightSpeed = 0.0_mps;
+ units::meters_per_second_t m_rearRightSpeed = 0.0_mps;
+
+ frc::ProfiledPIDController<units::radians> m_rotController{
+ 1, 0, 0,
+ frc::TrapezoidProfile<units::radians>::Constraints{
+ 9_rad_per_s, units::unit_t<radians_per_second_squared_t>(3)}};
+
+ static constexpr units::meter_t kxTolerance{1 / 12.0};
+ static constexpr units::meter_t kyTolerance{1 / 12.0};
+ static constexpr units::radian_t kAngularTolerance{1 / 12.0};
+
+ static constexpr units::meter_t kWheelBase{0.5};
+ static constexpr units::meter_t kTrackWidth{0.5};
+
+ frc::MecanumDriveKinematics m_kinematics{
+ frc::Translation2d{kWheelBase / 2, kTrackWidth / 2},
+ frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2},
+ frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2},
+ frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}};
+
+ frc::MecanumDriveOdometry m_odometry{m_kinematics, 0_rad,
+ frc::Pose2d{0_m, 0_m, 0_rad}};
+
+ frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
+ return frc::MecanumDriveWheelSpeeds{m_frontLeftSpeed, m_frontRightSpeed,
+ m_rearLeftSpeed, m_rearRightSpeed};
+ }
+
+ frc::Pose2d getRobotPose() {
+ m_odometry.UpdateWithTime(m_timer.Get(), m_angle, getCurrentWheelSpeeds());
+ return m_odometry.GetPose();
+ }
+};
+
+TEST_F(MecanumControllerCommandTest, ReachesReference) {
+ frc2::Subsystem subsystem{};
+
+ auto waypoints =
+ std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}};
+ auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ waypoints, {8.8_mps, 0.1_mps_sq});
+
+ auto endState = trajectory.Sample(trajectory.TotalTime());
+
+ auto command = frc2::MecanumControllerCommand(
+ trajectory, [&]() { return getRobotPose(); }, m_kinematics,
+
+ frc2::PIDController(0.6, 0, 0), frc2::PIDController(0.6, 0, 0),
+ m_rotController, units::meters_per_second_t(8.8),
+ [&](units::meters_per_second_t frontLeft,
+ units::meters_per_second_t rearLeft,
+ units::meters_per_second_t frontRight,
+ units::meters_per_second_t rearRight) {
+ m_frontLeftSpeed = frontLeft;
+ m_rearLeftSpeed = rearLeft;
+ m_frontRightSpeed = frontRight;
+ m_rearRightSpeed = rearRight;
+ },
+ {&subsystem});
+
+ m_timer.Reset();
+ m_timer.Start();
+ command.Initialize();
+ while (!command.IsFinished()) {
+ command.Execute();
+ m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
+ }
+ m_timer.Stop();
+ command.End(false);
+
+ EXPECT_NEAR_UNITS(endState.pose.Translation().X(),
+ getRobotPose().Translation().X(), kxTolerance);
+ EXPECT_NEAR_UNITS(endState.pose.Translation().Y(),
+ getRobotPose().Translation().Y(), kyTolerance);
+ EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(),
+ getRobotPose().Rotation().Radians(), kAngularTolerance);
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
similarity index 94%
rename from wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
index 695ccc3..a34c292 100644
--- a/wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
@@ -26,5 +26,6 @@
std::this_thread::sleep_for(std::chrono::milliseconds(250));
scheduler.Cancel(&command);
- EXPECT_NEAR(.01 * counter, .25, .025);
+ EXPECT_GT(counter, 10);
+ EXPECT_LT(counter, 100);
}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/PrintCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/PrintCommandTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/PrintCommandTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/PrintCommandTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/RunCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/RunCommandTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/RunCommandTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/RunCommandTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/SchedulerTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/SchedulerTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/SelectCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SelectCommandTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/SelectCommandTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/SelectCommandTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp
new file mode 100644
index 0000000..fa0838d
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc2/Timer.h>
+#include <frc2/command/Subsystem.h>
+#include <frc2/command/SwerveControllerCommand.h>
+
+#include <iostream>
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/kinematics/SwerveDriveOdometry.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <wpi/math>
+
+#include "gtest/gtest.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+ EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+class SwerveControllerCommandTest : public ::testing::Test {
+ using radians_per_second_squared_t =
+ units::compound_unit<units::radians,
+ units::inverse<units::squared<units::second>>>;
+
+ protected:
+ frc2::Timer m_timer;
+ frc::Rotation2d m_angle{0_rad};
+
+ std::array<frc::SwerveModuleState, 4> m_moduleStates{
+ frc::SwerveModuleState{}, frc::SwerveModuleState{},
+ frc::SwerveModuleState{}, frc::SwerveModuleState{}};
+
+ frc::ProfiledPIDController<units::radians> m_rotController{
+ 1, 0, 0,
+ frc::TrapezoidProfile<units::radians>::Constraints{
+ 9_rad_per_s, units::unit_t<radians_per_second_squared_t>(3)}};
+
+ static constexpr units::meter_t kxTolerance{1 / 12.0};
+ static constexpr units::meter_t kyTolerance{1 / 12.0};
+ static constexpr units::radian_t kAngularTolerance{1 / 12.0};
+
+ static constexpr units::meter_t kWheelBase{0.5};
+ static constexpr units::meter_t kTrackWidth{0.5};
+
+ frc::SwerveDriveKinematics<4> m_kinematics{
+ frc::Translation2d{kWheelBase / 2, kTrackWidth / 2},
+ frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2},
+ frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2},
+ frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}};
+
+ frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad,
+ frc::Pose2d{0_m, 0_m, 0_rad}};
+
+ std::array<frc::SwerveModuleState, 4> getCurrentWheelSpeeds() {
+ return m_moduleStates;
+ }
+
+ frc::Pose2d getRobotPose() {
+ m_odometry.UpdateWithTime(m_timer.Get(), m_angle, getCurrentWheelSpeeds());
+ return m_odometry.GetPose();
+ }
+};
+
+TEST_F(SwerveControllerCommandTest, ReachesReference) {
+ frc2::Subsystem subsystem{};
+
+ auto waypoints =
+ std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}};
+ auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ waypoints, {8.8_mps, 0.1_mps_sq});
+
+ auto endState = trajectory.Sample(trajectory.TotalTime());
+
+ auto command = frc2::SwerveControllerCommand<4>(
+ trajectory, [&]() { return getRobotPose(); }, m_kinematics,
+
+ frc2::PIDController(0.6, 0, 0), frc2::PIDController(0.6, 0, 0),
+ m_rotController,
+ [&](auto moduleStates) { m_moduleStates = moduleStates; }, {&subsystem});
+
+ m_timer.Reset();
+ m_timer.Start();
+ command.Initialize();
+ while (!command.IsFinished()) {
+ command.Execute();
+ m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
+ }
+ m_timer.Stop();
+ command.End(false);
+
+ EXPECT_NEAR_UNITS(endState.pose.Translation().X(),
+ getRobotPose().Translation().X(), kxTolerance);
+ EXPECT_NEAR_UNITS(endState.pose.Translation().Y(),
+ getRobotPose().Translation().Y(), kyTolerance);
+ EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(),
+ getRobotPose().Rotation().Radians(), kAngularTolerance);
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
similarity index 96%
rename from wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
index 3363975..75841a6 100644
--- a/wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
@@ -15,7 +15,7 @@
TEST_F(WaitCommandTest, WaitCommandScheduleTest) {
CommandScheduler scheduler = GetScheduler();
- WaitCommand command(.1_s);
+ WaitCommand command(100_ms);
scheduler.Schedule(&command);
scheduler.Run();
diff --git a/wpilibc/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp
diff --git a/wpilibc/src/test/native/cpp/frc2/command/make_vector.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h
similarity index 100%
rename from wpilibc/src/test/native/cpp/frc2/command/make_vector.h
rename to wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java b/wpilibNewCommands/src/test/native/cpp/main.cpp
similarity index 61%
copy from wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
copy to wpilibNewCommands/src/test/native/cpp/main.cpp
index df39cd1..c6b6c58 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
+++ b/wpilibNewCommands/src/test/native/cpp/main.cpp
@@ -1,16 +1,17 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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.command;
+#include <hal/HALBase.h>
-/**
- * A class to simulate a simple subsystem.
- */
-public class MockSubsystem extends Subsystem {
- @Override
- protected void initDefaultCommand() {}
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+ HAL_Initialize(500, 0);
+ ::testing::InitGoogleTest(&argc, argv);
+ int ret = RUN_ALL_TESTS();
+ return ret;
}
diff --git a/wpilibNewCommands/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension b/wpilibNewCommands/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension
new file mode 100644
index 0000000..daaab59
--- /dev/null
+++ b/wpilibNewCommands/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension
@@ -0,0 +1 @@
+edu.wpi.first.wpilibj2.MockHardwareExtension
diff --git a/wpilibOldCommands/WPILibOldCommands.json b/wpilibOldCommands/WPILibOldCommands.json
new file mode 100644
index 0000000..f9fbc4d
--- /dev/null
+++ b/wpilibOldCommands/WPILibOldCommands.json
@@ -0,0 +1,37 @@
+{
+ "fileName": "WPILibOldCommands.json",
+ "name": "WPILib-Old-Commands",
+ "version": "2020.0.0",
+ "uuid": "b066afc2-5c18-43c4-b758-43381fcb275e",
+ "mavenUrls": [],
+ "jsonUrl": "",
+ "javaDependencies": [
+ {
+ "groupId": "edu.wpi.first.wpilibOldCommands",
+ "artifactId": "wpilibOldCommands-java",
+ "version": "wpilib"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "edu.wpi.first.wpilibOldCommands",
+ "artifactId": "wpilibOldCommands-cpp",
+ "version": "wpilib",
+ "libName": "wpilibOldCommands",
+ "headerClassifier": "headers",
+ "sourcesClassifier": "sources",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "linuxathena",
+ "linuxraspbian",
+ "linuxaarch64bionic",
+ "windowsx86-64",
+ "windowsx86",
+ "linuxx86-64",
+ "osxx86-64"
+ ]
+ }
+ ]
+}
diff --git a/wpilibOldCommands/build.gradle b/wpilibOldCommands/build.gradle
new file mode 100644
index 0000000..e6e061d
--- /dev/null
+++ b/wpilibOldCommands/build.gradle
@@ -0,0 +1,95 @@
+ext {
+ nativeName = 'wpilibOldCommands'
+ devMain = 'edu.wpi.first.wpilibj.commands.DevMain'
+}
+
+evaluationDependsOn(':ntcore')
+evaluationDependsOn(':cscore')
+evaluationDependsOn(':hal')
+evaluationDependsOn(':wpilibc')
+evaluationDependsOn(':cameraserver')
+evaluationDependsOn(':wpilibj')
+
+apply from: "${rootDir}/shared/javacpp/setupBuild.gradle"
+
+dependencies {
+ implementation project(':wpiutil')
+ implementation project(':ntcore')
+ implementation project(':cscore')
+ implementation project(':hal')
+ implementation project(':wpilibj')
+ devImplementation project(':wpiutil')
+ devImplementation project(':ntcore')
+ devImplementation project(':cscore')
+ devImplementation project(':hal')
+ devImplementation project(':wpilibj')
+}
+
+nativeUtils.exportsConfigs {
+ wpilibOldCommands {
+ x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
+ '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
+ '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
+ '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+ x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
+ '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
+ '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
+ '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+ }
+}
+
+apply plugin: DisableBuildingGTest
+
+model {
+ components {}
+ binaries {
+ all {
+ if (!it.buildable || !(it instanceof NativeBinarySpec)) {
+ return
+ }
+ lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+ lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
+ project(':hal').addHalDependency(it, 'shared')
+ lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+
+ if (it.component.name == "${nativeName}Dev") {
+ lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
+ project(':hal').addHalJniDependency(it)
+ }
+
+ if (it instanceof GoogleTestTestSuiteBinarySpec) {
+ nativeUtils.useRequiredLibrary(it, 'opencv_shared')
+ lib project: ':cscore', library: 'cscore', linkage: 'shared'
+ }
+ if ((it instanceof NativeExecutableBinarySpec || it instanceof GoogleTestTestSuiteBinarySpec) && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
+ }
+ }
+ tasks {
+ def c = $.components
+ def found = false
+ def systemArch = getCurrentArch()
+ c.each {
+ if (it in NativeExecutableSpec && it.name == "${nativeName}Dev") {
+ it.binaries.each {
+ if (!found) {
+ def arch = it.targetPlatform.name
+ if (arch == systemArch) {
+ def filePath = it.tasks.install.installDirectory.get().toString() + File.separatorChar + 'lib'
+
+ found = true
+ }
+ }
+ }
+ }
+ }
+ }
+}
+
+test {
+ testLogging {
+ outputs.upToDateWhen {false}
+ showStandardStreams = true
+ }
+}
diff --git a/wpilibOldCommands/src/dev/java/edu/wpi/first/wpilibj/commands/DevMain.java b/wpilibOldCommands/src/dev/java/edu/wpi/first/wpilibj/commands/DevMain.java
new file mode 100644
index 0000000..5a5f6a9
--- /dev/null
+++ b/wpilibOldCommands/src/dev/java/edu/wpi/first/wpilibj/commands/DevMain.java
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* 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.commands;
+
+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/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java b/wpilibOldCommands/src/dev/native/cpp/main.cpp
similarity index 61%
copy from wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
copy to wpilibOldCommands/src/dev/native/cpp/main.cpp
index df39cd1..5312a1d 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
+++ b/wpilibOldCommands/src/dev/native/cpp/main.cpp
@@ -1,16 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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.command;
-
-/**
- * A class to simulate a simple subsystem.
- */
-public class MockSubsystem extends Subsystem {
- @Override
- protected void initDefaultCommand() {}
-}
+int main() {}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java
similarity index 96%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java
index a7dc2fa..dcd4401 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java
similarity index 95%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java
index bb1eff7..c2a6eb1 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java
similarity index 95%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java
index db0d4c6..8d2b20e 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java
similarity index 94%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java
index 2fa7924..3176977 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java
similarity index 96%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java
index abf8ca7..39c891d 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Command.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Command.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
similarity index 99%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
index 53b62ee..525e681 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
similarity index 95%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
index 59f6e9c..1053f55 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java
similarity index 97%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java
index 1f3c5de..f255e48 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java
similarity index 96%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java
index e717a47..78da002 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
similarity index 99%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
index 3d7fd98..8b60254 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
similarity index 99%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
index 6465eb1..1f30f38 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java
similarity index 94%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java
index 6199c6d..b8d7fed 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Set.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Set.java
similarity index 94%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Set.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Set.java
index 4b05467..6aac6d7 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Set.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Set.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java
similarity index 94%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java
index 6dd7db8..bd8c658 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java
similarity index 96%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java
index 6c9193b..386bb0e 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java
similarity index 94%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java
index 1af7c1f..0e1762a 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java
similarity index 94%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java
index 27795c0..118da6a 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java
similarity index 93%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java
rename to wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java
index 799fa8c..dadcab5 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibc/src/main/native/cpp/buttons/Button.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/Button.cpp
similarity index 92%
rename from wpilibc/src/main/native/cpp/buttons/Button.cpp
rename to wpilibOldCommands/src/main/native/cpp/buttons/Button.cpp
index 45ede65..57c86bd 100644
--- a/wpilibc/src/main/native/cpp/buttons/Button.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/Button.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/buttons/ButtonScheduler.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/ButtonScheduler.cpp
similarity index 91%
rename from wpilibc/src/main/native/cpp/buttons/ButtonScheduler.cpp
rename to wpilibOldCommands/src/main/native/cpp/buttons/ButtonScheduler.cpp
index 1e10255..f79c487 100644
--- a/wpilibc/src/main/native/cpp/buttons/ButtonScheduler.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/ButtonScheduler.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/buttons/CancelButtonScheduler.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
similarity index 93%
rename from wpilibc/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
rename to wpilibOldCommands/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
index 39d1d25..b0f4433 100644
--- a/wpilibc/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/buttons/HeldButtonScheduler.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
similarity index 93%
rename from wpilibc/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
rename to wpilibOldCommands/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
index feaa3c6..9abf8bc 100644
--- a/wpilibc/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/buttons/InternalButton.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/InternalButton.cpp
similarity index 92%
rename from wpilibc/src/main/native/cpp/buttons/InternalButton.cpp
rename to wpilibOldCommands/src/main/native/cpp/buttons/InternalButton.cpp
index cac67c4..6ff7971 100644
--- a/wpilibc/src/main/native/cpp/buttons/InternalButton.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/InternalButton.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/buttons/JoystickButton.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/JoystickButton.cpp
similarity index 91%
rename from wpilibc/src/main/native/cpp/buttons/JoystickButton.cpp
rename to wpilibOldCommands/src/main/native/cpp/buttons/JoystickButton.cpp
index 2c93be2..c1d5a29 100644
--- a/wpilibc/src/main/native/cpp/buttons/JoystickButton.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/JoystickButton.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/buttons/NetworkButton.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/NetworkButton.cpp
similarity index 94%
rename from wpilibc/src/main/native/cpp/buttons/NetworkButton.cpp
rename to wpilibOldCommands/src/main/native/cpp/buttons/NetworkButton.cpp
index 5e8b1e0..8bca356 100644
--- a/wpilibc/src/main/native/cpp/buttons/NetworkButton.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/NetworkButton.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/buttons/POVButton.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/POVButton.cpp
similarity index 91%
rename from wpilibc/src/main/native/cpp/buttons/POVButton.cpp
rename to wpilibOldCommands/src/main/native/cpp/buttons/POVButton.cpp
index 6729923..73e847a 100644
--- a/wpilibc/src/main/native/cpp/buttons/POVButton.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/POVButton.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibc/src/main/native/cpp/buttons/PressedButtonScheduler.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
similarity index 93%
rename from wpilibc/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
rename to wpilibOldCommands/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
index a964117..4a470f5 100644
--- a/wpilibc/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
similarity index 93%
rename from wpilibc/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
rename to wpilibOldCommands/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
index 6d67004..671c13e 100644
--- a/wpilibc/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
similarity index 93%
rename from wpilibc/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
rename to wpilibOldCommands/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
index b722d45..cefccb5 100644
--- a/wpilibc/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/buttons/Trigger.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/Trigger.cpp
similarity index 100%
rename from wpilibc/src/main/native/cpp/buttons/Trigger.cpp
rename to wpilibOldCommands/src/main/native/cpp/buttons/Trigger.cpp
diff --git a/wpilibc/src/main/native/cpp/commands/Command.cpp b/wpilibOldCommands/src/main/native/cpp/commands/Command.cpp
similarity index 100%
rename from wpilibc/src/main/native/cpp/commands/Command.cpp
rename to wpilibOldCommands/src/main/native/cpp/commands/Command.cpp
diff --git a/wpilibc/src/main/native/cpp/commands/CommandGroup.cpp b/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp
similarity index 98%
rename from wpilibc/src/main/native/cpp/commands/CommandGroup.cpp
rename to wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp
index eac1746..f64ba67 100644
--- a/wpilibc/src/main/native/cpp/commands/CommandGroup.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/commands/CommandGroupEntry.cpp b/wpilibOldCommands/src/main/native/cpp/commands/CommandGroupEntry.cpp
similarity index 93%
rename from wpilibc/src/main/native/cpp/commands/CommandGroupEntry.cpp
rename to wpilibOldCommands/src/main/native/cpp/commands/CommandGroupEntry.cpp
index 7a75f00..3441743 100644
--- a/wpilibc/src/main/native/cpp/commands/CommandGroupEntry.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/CommandGroupEntry.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/commands/ConditionalCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp
similarity index 96%
rename from wpilibc/src/main/native/cpp/commands/ConditionalCommand.cpp
rename to wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp
index c44f7ba..1c75c65 100644
--- a/wpilibc/src/main/native/cpp/commands/ConditionalCommand.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibc/src/main/native/cpp/commands/InstantCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/InstantCommand.cpp
similarity index 95%
rename from wpilibc/src/main/native/cpp/commands/InstantCommand.cpp
rename to wpilibOldCommands/src/main/native/cpp/commands/InstantCommand.cpp
index 88ebfde..445c5e0 100644
--- a/wpilibc/src/main/native/cpp/commands/InstantCommand.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/InstantCommand.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibc/src/main/native/cpp/commands/PIDCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/PIDCommand.cpp
similarity index 98%
rename from wpilibc/src/main/native/cpp/commands/PIDCommand.cpp
rename to wpilibOldCommands/src/main/native/cpp/commands/PIDCommand.cpp
index 875d8fe..90a05f4 100644
--- a/wpilibc/src/main/native/cpp/commands/PIDCommand.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/PIDCommand.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/commands/PIDSubsystem.cpp b/wpilibOldCommands/src/main/native/cpp/commands/PIDSubsystem.cpp
similarity index 97%
rename from wpilibc/src/main/native/cpp/commands/PIDSubsystem.cpp
rename to wpilibOldCommands/src/main/native/cpp/commands/PIDSubsystem.cpp
index 0893da3..4d35943 100644
--- a/wpilibc/src/main/native/cpp/commands/PIDSubsystem.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/PIDSubsystem.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/commands/PrintCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/PrintCommand.cpp
similarity index 91%
rename from wpilibc/src/main/native/cpp/commands/PrintCommand.cpp
rename to wpilibOldCommands/src/main/native/cpp/commands/PrintCommand.cpp
index b4bea24..888e8a1 100644
--- a/wpilibc/src/main/native/cpp/commands/PrintCommand.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/PrintCommand.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/commands/Scheduler.cpp b/wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp
similarity index 99%
rename from wpilibc/src/main/native/cpp/commands/Scheduler.cpp
rename to wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp
index f406d74..6944e41 100644
--- a/wpilibc/src/main/native/cpp/commands/Scheduler.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp
@@ -12,7 +12,7 @@
#include <string>
#include <vector>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <networktables/NetworkTableEntry.h>
#include <wpi/mutex.h>
diff --git a/wpilibc/src/main/native/cpp/commands/StartCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/StartCommand.cpp
similarity index 91%
rename from wpilibc/src/main/native/cpp/commands/StartCommand.cpp
rename to wpilibOldCommands/src/main/native/cpp/commands/StartCommand.cpp
index 8884124..97ddfb9 100644
--- a/wpilibc/src/main/native/cpp/commands/StartCommand.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/StartCommand.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/commands/Subsystem.cpp b/wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp
similarity index 100%
rename from wpilibc/src/main/native/cpp/commands/Subsystem.cpp
rename to wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp
diff --git a/wpilibc/src/main/native/cpp/commands/TimedCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/TimedCommand.cpp
similarity index 93%
rename from wpilibc/src/main/native/cpp/commands/TimedCommand.cpp
rename to wpilibOldCommands/src/main/native/cpp/commands/TimedCommand.cpp
index 122751a..f5113b2 100644
--- a/wpilibc/src/main/native/cpp/commands/TimedCommand.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/TimedCommand.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibc/src/main/native/cpp/commands/WaitCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/WaitCommand.cpp
similarity index 91%
rename from wpilibc/src/main/native/cpp/commands/WaitCommand.cpp
rename to wpilibOldCommands/src/main/native/cpp/commands/WaitCommand.cpp
index 225d95b..d326193 100644
--- a/wpilibc/src/main/native/cpp/commands/WaitCommand.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/WaitCommand.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/commands/WaitForChildren.cpp b/wpilibOldCommands/src/main/native/cpp/commands/WaitForChildren.cpp
similarity index 92%
rename from wpilibc/src/main/native/cpp/commands/WaitForChildren.cpp
rename to wpilibOldCommands/src/main/native/cpp/commands/WaitForChildren.cpp
index 5c99c1b..374c11c 100644
--- a/wpilibc/src/main/native/cpp/commands/WaitForChildren.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/WaitForChildren.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/cpp/commands/WaitUntilCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/WaitUntilCommand.cpp
similarity index 92%
rename from wpilibc/src/main/native/cpp/commands/WaitUntilCommand.cpp
rename to wpilibOldCommands/src/main/native/cpp/commands/WaitUntilCommand.cpp
index 6e24b6b..07ff00b 100644
--- a/wpilibc/src/main/native/cpp/commands/WaitUntilCommand.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/WaitUntilCommand.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/buttons/Button.h b/wpilibOldCommands/src/main/native/include/frc/buttons/Button.h
similarity index 96%
rename from wpilibc/src/main/native/include/frc/buttons/Button.h
rename to wpilibOldCommands/src/main/native/include/frc/buttons/Button.h
index 71641e9..c0830bf 100644
--- a/wpilibc/src/main/native/include/frc/buttons/Button.h
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/Button.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/buttons/ButtonScheduler.h b/wpilibOldCommands/src/main/native/include/frc/buttons/ButtonScheduler.h
similarity index 92%
rename from wpilibc/src/main/native/include/frc/buttons/ButtonScheduler.h
rename to wpilibOldCommands/src/main/native/include/frc/buttons/ButtonScheduler.h
index ec2e35b..960fbc8 100644
--- a/wpilibc/src/main/native/include/frc/buttons/ButtonScheduler.h
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/ButtonScheduler.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/buttons/CancelButtonScheduler.h b/wpilibOldCommands/src/main/native/include/frc/buttons/CancelButtonScheduler.h
similarity index 93%
rename from wpilibc/src/main/native/include/frc/buttons/CancelButtonScheduler.h
rename to wpilibOldCommands/src/main/native/include/frc/buttons/CancelButtonScheduler.h
index 69b8a11..9131fc0 100644
--- a/wpilibc/src/main/native/include/frc/buttons/CancelButtonScheduler.h
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/CancelButtonScheduler.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/buttons/HeldButtonScheduler.h b/wpilibOldCommands/src/main/native/include/frc/buttons/HeldButtonScheduler.h
similarity index 92%
rename from wpilibc/src/main/native/include/frc/buttons/HeldButtonScheduler.h
rename to wpilibOldCommands/src/main/native/include/frc/buttons/HeldButtonScheduler.h
index ad4a160..2b51ce6 100644
--- a/wpilibc/src/main/native/include/frc/buttons/HeldButtonScheduler.h
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/HeldButtonScheduler.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/buttons/InternalButton.h b/wpilibOldCommands/src/main/native/include/frc/buttons/InternalButton.h
similarity index 93%
rename from wpilibc/src/main/native/include/frc/buttons/InternalButton.h
rename to wpilibOldCommands/src/main/native/include/frc/buttons/InternalButton.h
index 430a21e..ed4dc56 100644
--- a/wpilibc/src/main/native/include/frc/buttons/InternalButton.h
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/InternalButton.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/buttons/JoystickButton.h b/wpilibOldCommands/src/main/native/include/frc/buttons/JoystickButton.h
similarity index 92%
rename from wpilibc/src/main/native/include/frc/buttons/JoystickButton.h
rename to wpilibOldCommands/src/main/native/include/frc/buttons/JoystickButton.h
index 1b4264f..e0e2d4c 100644
--- a/wpilibc/src/main/native/include/frc/buttons/JoystickButton.h
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/JoystickButton.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/buttons/NetworkButton.h b/wpilibOldCommands/src/main/native/include/frc/buttons/NetworkButton.h
similarity index 94%
rename from wpilibc/src/main/native/include/frc/buttons/NetworkButton.h
rename to wpilibOldCommands/src/main/native/include/frc/buttons/NetworkButton.h
index fef8065..f16821b 100644
--- a/wpilibc/src/main/native/include/frc/buttons/NetworkButton.h
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/NetworkButton.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/buttons/POVButton.h b/wpilibOldCommands/src/main/native/include/frc/buttons/POVButton.h
similarity index 94%
rename from wpilibc/src/main/native/include/frc/buttons/POVButton.h
rename to wpilibOldCommands/src/main/native/include/frc/buttons/POVButton.h
index 15c4bf8..bd73984 100644
--- a/wpilibc/src/main/native/include/frc/buttons/POVButton.h
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/POVButton.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibc/src/main/native/include/frc/buttons/PressedButtonScheduler.h b/wpilibOldCommands/src/main/native/include/frc/buttons/PressedButtonScheduler.h
similarity index 93%
rename from wpilibc/src/main/native/include/frc/buttons/PressedButtonScheduler.h
rename to wpilibOldCommands/src/main/native/include/frc/buttons/PressedButtonScheduler.h
index 0c1fb03..29c6a95 100644
--- a/wpilibc/src/main/native/include/frc/buttons/PressedButtonScheduler.h
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/PressedButtonScheduler.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h b/wpilibOldCommands/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h
similarity index 93%
rename from wpilibc/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h
rename to wpilibOldCommands/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h
index 899a483..a567a7b 100644
--- a/wpilibc/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/buttons/ToggleButtonScheduler.h b/wpilibOldCommands/src/main/native/include/frc/buttons/ToggleButtonScheduler.h
similarity index 93%
rename from wpilibc/src/main/native/include/frc/buttons/ToggleButtonScheduler.h
rename to wpilibOldCommands/src/main/native/include/frc/buttons/ToggleButtonScheduler.h
index aeb93b3..406d131 100644
--- a/wpilibc/src/main/native/include/frc/buttons/ToggleButtonScheduler.h
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/ToggleButtonScheduler.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/buttons/Trigger.h b/wpilibOldCommands/src/main/native/include/frc/buttons/Trigger.h
similarity index 100%
rename from wpilibc/src/main/native/include/frc/buttons/Trigger.h
rename to wpilibOldCommands/src/main/native/include/frc/buttons/Trigger.h
diff --git a/wpilibc/src/main/native/include/frc/commands/Command.h b/wpilibOldCommands/src/main/native/include/frc/commands/Command.h
similarity index 99%
rename from wpilibc/src/main/native/include/frc/commands/Command.h
rename to wpilibOldCommands/src/main/native/include/frc/commands/Command.h
index d40bb00..241490a 100644
--- a/wpilibc/src/main/native/include/frc/commands/Command.h
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/Command.h
@@ -388,6 +388,7 @@
friend class ConditionalCommand;
+ public:
/**
* Gets the name of this Command.
*
diff --git a/wpilibc/src/main/native/include/frc/commands/CommandGroup.h b/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroup.h
similarity index 98%
rename from wpilibc/src/main/native/include/frc/commands/CommandGroup.h
rename to wpilibOldCommands/src/main/native/include/frc/commands/CommandGroup.h
index 690ae6d..0275cb5 100644
--- a/wpilibc/src/main/native/include/frc/commands/CommandGroup.h
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroup.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/commands/CommandGroupEntry.h b/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroupEntry.h
similarity index 93%
rename from wpilibc/src/main/native/include/frc/commands/CommandGroupEntry.h
rename to wpilibOldCommands/src/main/native/include/frc/commands/CommandGroupEntry.h
index 2de1e43..d1d2264 100644
--- a/wpilibc/src/main/native/include/frc/commands/CommandGroupEntry.h
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroupEntry.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/commands/ConditionalCommand.h b/wpilibOldCommands/src/main/native/include/frc/commands/ConditionalCommand.h
similarity index 100%
rename from wpilibc/src/main/native/include/frc/commands/ConditionalCommand.h
rename to wpilibOldCommands/src/main/native/include/frc/commands/ConditionalCommand.h
diff --git a/wpilibc/src/main/native/include/frc/commands/InstantCommand.h b/wpilibOldCommands/src/main/native/include/frc/commands/InstantCommand.h
similarity index 97%
rename from wpilibc/src/main/native/include/frc/commands/InstantCommand.h
rename to wpilibOldCommands/src/main/native/include/frc/commands/InstantCommand.h
index 987dc18..3663b7f 100644
--- a/wpilibc/src/main/native/include/frc/commands/InstantCommand.h
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/InstantCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibc/src/main/native/include/frc/commands/PIDCommand.h b/wpilibOldCommands/src/main/native/include/frc/commands/PIDCommand.h
similarity index 97%
rename from wpilibc/src/main/native/include/frc/commands/PIDCommand.h
rename to wpilibOldCommands/src/main/native/include/frc/commands/PIDCommand.h
index 02f1710..d00ac9b 100644
--- a/wpilibc/src/main/native/include/frc/commands/PIDCommand.h
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/PIDCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/commands/PIDSubsystem.h b/wpilibOldCommands/src/main/native/include/frc/commands/PIDSubsystem.h
similarity index 98%
rename from wpilibc/src/main/native/include/frc/commands/PIDSubsystem.h
rename to wpilibOldCommands/src/main/native/include/frc/commands/PIDSubsystem.h
index 1965492..2afc05b 100644
--- a/wpilibc/src/main/native/include/frc/commands/PIDSubsystem.h
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/PIDSubsystem.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/commands/PrintCommand.h b/wpilibOldCommands/src/main/native/include/frc/commands/PrintCommand.h
similarity index 92%
rename from wpilibc/src/main/native/include/frc/commands/PrintCommand.h
rename to wpilibOldCommands/src/main/native/include/frc/commands/PrintCommand.h
index 13156c9..5e987d3 100644
--- a/wpilibc/src/main/native/include/frc/commands/PrintCommand.h
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/PrintCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/commands/Scheduler.h b/wpilibOldCommands/src/main/native/include/frc/commands/Scheduler.h
similarity index 100%
rename from wpilibc/src/main/native/include/frc/commands/Scheduler.h
rename to wpilibOldCommands/src/main/native/include/frc/commands/Scheduler.h
diff --git a/wpilibc/src/main/native/include/frc/commands/StartCommand.h b/wpilibOldCommands/src/main/native/include/frc/commands/StartCommand.h
similarity index 92%
rename from wpilibc/src/main/native/include/frc/commands/StartCommand.h
rename to wpilibOldCommands/src/main/native/include/frc/commands/StartCommand.h
index 4f0b676..5062f92 100644
--- a/wpilibc/src/main/native/include/frc/commands/StartCommand.h
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/StartCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/commands/Subsystem.h b/wpilibOldCommands/src/main/native/include/frc/commands/Subsystem.h
similarity index 100%
rename from wpilibc/src/main/native/include/frc/commands/Subsystem.h
rename to wpilibOldCommands/src/main/native/include/frc/commands/Subsystem.h
diff --git a/wpilibc/src/main/native/include/frc/commands/TimedCommand.h b/wpilibOldCommands/src/main/native/include/frc/commands/TimedCommand.h
similarity index 96%
rename from wpilibc/src/main/native/include/frc/commands/TimedCommand.h
rename to wpilibOldCommands/src/main/native/include/frc/commands/TimedCommand.h
index d2be010..16f3348 100644
--- a/wpilibc/src/main/native/include/frc/commands/TimedCommand.h
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/TimedCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibc/src/main/native/include/frc/commands/WaitCommand.h b/wpilibOldCommands/src/main/native/include/frc/commands/WaitCommand.h
similarity index 94%
rename from wpilibc/src/main/native/include/frc/commands/WaitCommand.h
rename to wpilibOldCommands/src/main/native/include/frc/commands/WaitCommand.h
index 481b1c6..794b0f7 100644
--- a/wpilibc/src/main/native/include/frc/commands/WaitCommand.h
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/WaitCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/commands/WaitForChildren.h b/wpilibOldCommands/src/main/native/include/frc/commands/WaitForChildren.h
similarity index 92%
rename from wpilibc/src/main/native/include/frc/commands/WaitForChildren.h
rename to wpilibOldCommands/src/main/native/include/frc/commands/WaitForChildren.h
index 6f2e285..cd1f85a 100644
--- a/wpilibc/src/main/native/include/frc/commands/WaitForChildren.h
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/WaitForChildren.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibc/src/main/native/include/frc/commands/WaitUntilCommand.h b/wpilibOldCommands/src/main/native/include/frc/commands/WaitUntilCommand.h
similarity index 94%
rename from wpilibc/src/main/native/include/frc/commands/WaitUntilCommand.h
rename to wpilibOldCommands/src/main/native/include/frc/commands/WaitUntilCommand.h
index 1231750..b2f1ffe 100644
--- a/wpilibc/src/main/native/include/frc/commands/WaitUntilCommand.h
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/WaitUntilCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
new file mode 100644
index 0000000..d97b352
--- /dev/null
+++ b/wpilibOldCommands/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/command/AbstractCommandTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/AbstractCommandTest.java
similarity index 96%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/command/AbstractCommandTest.java
rename to wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/AbstractCommandTest.java
index 1fa9aa8..409a74f 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/AbstractCommandTest.java
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/AbstractCommandTest.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ButtonTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ButtonTest.java
similarity index 98%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ButtonTest.java
rename to wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ButtonTest.java
index 3686c78..690eade 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ButtonTest.java
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ButtonTest.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java
rename to wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandScheduleTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandScheduleTest.java
similarity index 96%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandScheduleTest.java
rename to wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandScheduleTest.java
index feb80bc..5711032 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandScheduleTest.java
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandScheduleTest.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java
similarity index 100%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java
rename to wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandSupersedeTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSupersedeTest.java
similarity index 98%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandSupersedeTest.java
rename to wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSupersedeTest.java
index 4ba965f..91ec0de 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandSupersedeTest.java
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSupersedeTest.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandTimeoutTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandTimeoutTest.java
similarity index 95%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandTimeoutTest.java
rename to wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandTimeoutTest.java
index ff7ad86..229742c 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandTimeoutTest.java
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandTimeoutTest.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java
similarity index 99%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java
rename to wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java
index 5f3285c..76ec10b 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/DefaultCommandTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/DefaultCommandTest.java
similarity index 98%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/command/DefaultCommandTest.java
rename to wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/DefaultCommandTest.java
index 1cfec7f..3ec2148 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/DefaultCommandTest.java
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/DefaultCommandTest.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java
similarity index 97%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java
rename to wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java
index bee2f80..82fe3e2 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java
similarity index 97%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java
rename to wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java
index 876f1c5..688afa3 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
similarity index 90%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
rename to wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
index df39cd1..604df89 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java
similarity index 98%
rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java
rename to wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java
index fe48c13..16f088f 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java b/wpilibOldCommands/src/test/native/cpp/main.cpp
similarity index 61%
copy from wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
copy to wpilibOldCommands/src/test/native/cpp/main.cpp
index df39cd1..c6b6c58 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
+++ b/wpilibOldCommands/src/test/native/cpp/main.cpp
@@ -1,16 +1,17 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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.command;
+#include <hal/HALBase.h>
-/**
- * A class to simulate a simple subsystem.
- */
-public class MockSubsystem extends Subsystem {
- @Override
- protected void initDefaultCommand() {}
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+ HAL_Initialize(500, 0);
+ ::testing::InitGoogleTest(&argc, argv);
+ int ret = RUN_ALL_TESTS();
+ return ret;
}
diff --git a/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp b/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp
similarity index 98%
rename from wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp
rename to wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp
index 23f3e3a..e0e3db0 100644
--- a/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp
+++ b/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp b/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp
similarity index 100%
rename from wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp
rename to wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp
diff --git a/wpilibOldCommands/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension b/wpilibOldCommands/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension
new file mode 100644
index 0000000..981f170
--- /dev/null
+++ b/wpilibOldCommands/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension
@@ -0,0 +1 @@
+edu.wpi.first.wpilibj.MockHardwareExtension
diff --git a/wpilibc/build.gradle b/wpilibc/build.gradle
index 5f93f7e..8730824 100644
--- a/wpilibc/build.gradle
+++ b/wpilibc/build.gradle
@@ -50,18 +50,6 @@
}
}
-ext {
- sharedCvConfigs = [wpilibc : [],
- wpilibcBase: [],
- wpilibcDev : [],
- wpilibcTest: []]
- staticCvConfigs = [:]
- useJava = false
- useCpp = true
-}
-
-apply from: "${rootDir}/shared/opencv.gradle"
-
project(':').libraryBuild.dependsOn build
ext {
@@ -72,16 +60,6 @@
apply from: "${rootDir}/shared/googletest.gradle"
-ext {
- chipObjectComponents = ["$nativeName".toString(), "${nativeName}Dev".toString(), "${nativeName}Base".toString(),
- "${nativeName}Test".toString()]
- netCommComponents = ["$nativeName".toString(), "${nativeName}Dev".toString(), "${nativeName}Base".toString(),
- "${nativeName}Test".toString()]
- useNiJava = false
-}
-
-apply from: "${rootDir}/shared/nilibraries.gradle"
-
apply plugin: DisableBuildingGTest
nativeUtils.exportsConfigs {
@@ -147,6 +125,7 @@
} else {
lib project: ':cscore', library: 'cscore', linkage: 'shared'
lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+ nativeUtils.useRequiredLibrary(it, 'opencv_shared')
}
}
@@ -174,6 +153,10 @@
project(':hal').addHalDependency(it, 'shared')
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+ nativeUtils.useRequiredLibrary(it, 'opencv_shared')
+ if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
}
}
}
@@ -219,7 +202,11 @@
project(':hal').addHalDependency(it, 'shared')
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+ nativeUtils.useRequiredLibrary(it, 'opencv_shared')
lib library: nativeName, linkage: 'shared'
+ if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
}
}
tasks {
diff --git a/wpilibc/publish.gradle b/wpilibc/publish.gradle
index dd7601e..025c09d 100644
--- a/wpilibc/publish.gradle
+++ b/wpilibc/publish.gradle
@@ -7,8 +7,8 @@
def outputsFolder = file("$project.buildDir/outputs")
task cppSourcesZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = zipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = zipBaseName
classifier = "sources"
from(licenseFile) {
@@ -26,8 +26,8 @@
cppSourcesZip.dependsOn generateCppVersion
task cppHeadersZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = zipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = zipBaseName
classifier = "headers"
from(licenseFile) {
diff --git a/wpilibc/src/dev/native/cpp/main.cpp b/wpilibc/src/dev/native/cpp/main.cpp
index 6e64d2b..f1370bb 100644
--- a/wpilibc/src/dev/native/cpp/main.cpp
+++ b/wpilibc/src/dev/native/cpp/main.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -7,7 +7,7 @@
#include <iostream>
-#include <hal/HAL.h>
+#include <hal/HALBase.h>
#include "WPILibVersion.h"
diff --git a/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
index 5b5762a..a760daa 100644
--- a/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
@@ -7,7 +7,7 @@
#include "frc/ADXL345_I2C.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableBuilder.h"
#include "frc/smartdashboard/SendableRegistry.h"
diff --git a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
index bb9275a..077a7ff 100644
--- a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
@@ -7,7 +7,7 @@
#include "frc/ADXL345_SPI.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableBuilder.h"
#include "frc/smartdashboard/SendableRegistry.h"
diff --git a/wpilibc/src/main/native/cpp/ADXL362.cpp b/wpilibc/src/main/native/cpp/ADXL362.cpp
index 2f97f1b..6ed8c8c 100644
--- a/wpilibc/src/main/native/cpp/ADXL362.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL362.cpp
@@ -7,7 +7,7 @@
#include "frc/ADXL362.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/DriverStation.h"
#include "frc/smartdashboard/SendableBuilder.h"
@@ -72,7 +72,7 @@
commands[2] = kPowerCtl_Measure | kPowerCtl_UltraLowNoise;
m_spi.Write(commands, 3);
- HAL_Report(HALUsageReporting::kResourceType_ADXL362, port);
+ HAL_Report(HALUsageReporting::kResourceType_ADXL362, port + 1);
SendableRegistry::GetInstance().AddLW(this, "ADXL362", port);
}
diff --git a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
index 3dde234..2cf2f73 100644
--- a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
+++ b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
@@ -7,7 +7,7 @@
#include "frc/ADXRS450_Gyro.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/DriverStation.h"
#include "frc/Timer.h"
@@ -57,7 +57,7 @@
Calibrate();
}
- HAL_Report(HALUsageReporting::kResourceType_ADXRS450, port);
+ HAL_Report(HALUsageReporting::kResourceType_ADXRS450, port + 1);
SendableRegistry::GetInstance().AddLW(this, "ADXRS450_Gyro", port);
}
diff --git a/wpilibc/src/main/native/cpp/AddressableLED.cpp b/wpilibc/src/main/native/cpp/AddressableLED.cpp
new file mode 100644
index 0000000..b0c3a45
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AddressableLED.cpp
@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/AddressableLED.h"
+
+#include <hal/AddressableLED.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <hal/PWM.h>
+#include <hal/Ports.h>
+
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+AddressableLED::AddressableLED(int port) {
+ int32_t status = 0;
+
+ m_pwmHandle = HAL_InitializePWMPort(HAL_GetPort(port), &status);
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumPWMChannels(), port);
+ if (m_pwmHandle == HAL_kInvalidHandle) {
+ return;
+ }
+
+ m_handle = HAL_InitializeAddressableLED(m_pwmHandle, &status);
+ wpi_setHALError(status);
+ if (m_handle == HAL_kInvalidHandle) {
+ HAL_FreePWMPort(m_pwmHandle, &status);
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_AddressableLEDs, port + 1);
+}
+
+AddressableLED::~AddressableLED() {
+ HAL_FreeAddressableLED(m_handle);
+ int32_t status = 0;
+ HAL_FreePWMPort(m_pwmHandle, &status);
+}
+
+void AddressableLED::SetLength(int length) {
+ int32_t status = 0;
+ HAL_SetAddressableLEDLength(m_handle, length, &status);
+ wpi_setHALError(status);
+}
+
+static_assert(sizeof(AddressableLED::LEDData) == sizeof(HAL_AddressableLEDData),
+ "LED Structs MUST be the same size");
+
+void AddressableLED::SetData(wpi::ArrayRef<LEDData> ledData) {
+ int32_t status = 0;
+ HAL_WriteAddressableLEDData(m_handle, ledData.begin(), ledData.size(),
+ &status);
+ wpi_setHALError(status);
+}
+
+void AddressableLED::SetData(std::initializer_list<LEDData> ledData) {
+ int32_t status = 0;
+ HAL_WriteAddressableLEDData(m_handle, ledData.begin(), ledData.size(),
+ &status);
+ wpi_setHALError(status);
+}
+
+void AddressableLED::SetBitTiming(units::nanosecond_t lowTime0,
+ units::nanosecond_t highTime0,
+ units::nanosecond_t lowTime1,
+ units::nanosecond_t highTime1) {
+ int32_t status = 0;
+ HAL_SetAddressableLEDBitTiming(
+ m_handle, lowTime0.to<int32_t>(), highTime0.to<int32_t>(),
+ lowTime1.to<int32_t>(), highTime1.to<int32_t>(), &status);
+ wpi_setHALError(status);
+}
+
+void AddressableLED::SetSyncTime(units::microsecond_t syncTime) {
+ int32_t status = 0;
+ HAL_SetAddressableLEDSyncTime(m_handle, syncTime.to<int32_t>(), &status);
+ wpi_setHALError(status);
+}
+
+void AddressableLED::Start() {
+ int32_t status = 0;
+ HAL_StartAddressableLEDOutput(m_handle, &status);
+ wpi_setHALError(status);
+}
+
+void AddressableLED::Stop() {
+ int32_t status = 0;
+ HAL_StopAddressableLEDOutput(m_handle, &status);
+ wpi_setHALError(status);
+}
+
+void AddressableLED::LEDData::SetHSV(int h, int s, int v) {
+ if (s == 0) {
+ SetRGB(v, v, v);
+ return;
+ }
+
+ int region = h / 30;
+ int remainder = (h - (region * 30)) * 6;
+
+ int p = (v * (255 - s)) >> 8;
+ int q = (v * (255 - ((s * remainder) >> 8))) >> 8;
+ int t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8;
+
+ switch (region) {
+ case 0:
+ SetRGB(v, t, p);
+ break;
+ case 1:
+ SetRGB(q, v, p);
+ break;
+ case 2:
+ SetRGB(p, v, t);
+ break;
+ case 3:
+ SetRGB(p, q, v);
+ break;
+ case 4:
+ SetRGB(t, p, v);
+ break;
+ default:
+ SetRGB(v, p, q);
+ break;
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
index 4436bcf..be0c7d2 100644
--- a/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
@@ -7,7 +7,7 @@
#include "frc/AnalogAccelerometer.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
@@ -58,7 +58,7 @@
void AnalogAccelerometer::InitAccelerometer() {
HAL_Report(HALUsageReporting::kResourceType_Accelerometer,
- m_analogInput->GetChannel());
+ m_analogInput->GetChannel() + 1);
SendableRegistry::GetInstance().AddLW(this, "Accelerometer",
m_analogInput->GetChannel());
diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
new file mode 100644
index 0000000..a194961
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/AnalogEncoder.h"
+
+#include "frc/AnalogInput.h"
+#include "frc/Counter.h"
+#include "frc/DriverStation.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+AnalogEncoder::AnalogEncoder(AnalogInput& analogInput)
+ : m_analogInput{&analogInput, NullDeleter<AnalogInput>{}},
+ m_analogTrigger{m_analogInput.get()},
+ m_counter{} {
+ Init();
+}
+
+AnalogEncoder::AnalogEncoder(AnalogInput* analogInput)
+ : m_analogInput{analogInput, NullDeleter<AnalogInput>{}},
+ m_analogTrigger{m_analogInput.get()},
+ m_counter{} {
+ Init();
+}
+
+AnalogEncoder::AnalogEncoder(std::shared_ptr<AnalogInput> analogInput)
+ : m_analogInput{std::move(analogInput)},
+ m_analogTrigger{m_analogInput.get()},
+ m_counter{} {
+ Init();
+}
+
+void AnalogEncoder::Init() {
+ m_simDevice = hal::SimDevice{"AnalogEncoder", m_analogInput->GetChannel()};
+
+ if (m_simDevice) {
+ m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0);
+ }
+
+ m_analogTrigger.SetLimitsVoltage(1.25, 3.75);
+ m_counter.SetUpSource(
+ m_analogTrigger.CreateOutput(AnalogTriggerType::kRisingPulse));
+ m_counter.SetDownSource(
+ m_analogTrigger.CreateOutput(AnalogTriggerType::kFallingPulse));
+
+ SendableRegistry::GetInstance().AddLW(this, "DutyCycle Encoder",
+ m_analogInput->GetChannel());
+}
+
+units::turn_t AnalogEncoder::Get() const {
+ if (m_simPosition) return units::turn_t{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++) {
+ auto counter = m_counter.Get();
+ auto pos = m_analogInput->GetVoltage();
+ auto counter2 = m_counter.Get();
+ auto pos2 = m_analogInput->GetVoltage();
+ if (counter == counter2 && pos == pos2) {
+ units::turn_t turns{counter + pos - m_positionOffset};
+ m_lastPosition = turns;
+ return turns;
+ }
+ }
+
+ frc::DriverStation::GetInstance().ReportWarning(
+ "Failed to read Analog Encoder. Potential Speed Overrun. Returning last "
+ "value");
+ return m_lastPosition;
+}
+
+double AnalogEncoder::GetPositionOffset() const { return m_positionOffset; }
+
+void AnalogEncoder::SetDistancePerRotation(double distancePerRotation) {
+ m_distancePerRotation = distancePerRotation;
+}
+
+double AnalogEncoder::GetDistancePerRotation() const {
+ return m_distancePerRotation;
+}
+
+double AnalogEncoder::GetDistance() const {
+ return Get().to<double>() * GetDistancePerRotation();
+}
+
+void AnalogEncoder::Reset() {
+ m_counter.Reset();
+ m_positionOffset = m_analogInput->GetVoltage();
+}
+
+void AnalogEncoder::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("AbsoluteEncoder");
+ builder.AddDoubleProperty("Distance", [this] { return this->GetDistance(); },
+ nullptr);
+ builder.AddDoubleProperty("Distance Per Rotation",
+ [this] { return this->GetDistancePerRotation(); },
+ nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/AnalogGyro.cpp b/wpilibc/src/main/native/cpp/AnalogGyro.cpp
index 36cde8a..c891506 100644
--- a/wpilibc/src/main/native/cpp/AnalogGyro.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogGyro.cpp
@@ -12,7 +12,7 @@
#include <hal/AnalogGyro.h>
#include <hal/Errors.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/AnalogInput.h"
#include "frc/Timer.h"
@@ -56,7 +56,7 @@
HAL_SetAnalogGyroParameters(m_gyroHandle, kDefaultVoltsPerDegreePerSecond,
offset, center, &status);
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_gyroHandle = HAL_kInvalidHandle;
return;
}
@@ -70,7 +70,7 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double value = HAL_GetAnalogGyroAngle(m_gyroHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -78,7 +78,7 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double value = HAL_GetAnalogGyroRate(m_gyroHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -86,7 +86,7 @@
if (StatusIsFatal()) return 0;
int32_t status = 0;
int value = HAL_GetAnalogGyroCenter(m_gyroHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -94,7 +94,7 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double value = HAL_GetAnalogGyroOffset(m_gyroHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -102,21 +102,21 @@
int32_t status = 0;
HAL_SetAnalogGyroVoltsPerDegreePerSecond(m_gyroHandle,
voltsPerDegreePerSecond, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void AnalogGyro::SetDeadband(double volts) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogGyroDeadband(m_gyroHandle, volts, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void AnalogGyro::Reset() {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_ResetAnalogGyro(m_gyroHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void AnalogGyro::InitGyro() {
@@ -132,7 +132,7 @@
return;
}
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_analog = nullptr;
m_gyroHandle = HAL_kInvalidHandle;
return;
@@ -142,13 +142,13 @@
int32_t status = 0;
HAL_SetupAnalogGyro(m_gyroHandle, &status);
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_analog = nullptr;
m_gyroHandle = HAL_kInvalidHandle;
return;
}
- HAL_Report(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel() + 1);
SendableRegistry::GetInstance().AddLW(this, "AnalogGyro",
m_analog->GetChannel());
@@ -158,5 +158,5 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_CalibrateAnalogGyro(m_gyroHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
diff --git a/wpilibc/src/main/native/cpp/AnalogInput.cpp b/wpilibc/src/main/native/cpp/AnalogInput.cpp
index a200f4d..c8197b7 100644
--- a/wpilibc/src/main/native/cpp/AnalogInput.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogInput.cpp
@@ -11,7 +11,8 @@
#include <hal/AnalogAccumulator.h>
#include <hal/AnalogInput.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/Ports.h>
#include "frc/SensorUtil.h"
@@ -35,14 +36,13 @@
int32_t status = 0;
m_port = HAL_InitializeAnalogInputPort(port, &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumAnalogInputs(), channel,
- HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumAnalogInputs(), channel);
m_channel = std::numeric_limits<int>::max();
m_port = HAL_kInvalidHandle;
return;
}
- HAL_Report(HALUsageReporting::kResourceType_AnalogChannel, channel);
+ HAL_Report(HALUsageReporting::kResourceType_AnalogChannel, channel + 1);
SendableRegistry::GetInstance().AddLW(this, "AnalogInput", channel);
}
@@ -53,7 +53,7 @@
if (StatusIsFatal()) return 0;
int32_t status = 0;
int value = HAL_GetAnalogValue(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -61,7 +61,7 @@
if (StatusIsFatal()) return 0;
int32_t status = 0;
int value = HAL_GetAnalogAverageValue(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -69,7 +69,7 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double voltage = HAL_GetAnalogVoltage(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return voltage;
}
@@ -77,7 +77,7 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double voltage = HAL_GetAnalogAverageVoltage(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return voltage;
}
@@ -90,13 +90,13 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogAverageBits(m_port, bits, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int AnalogInput::GetAverageBits() const {
int32_t status = 0;
int averageBits = HAL_GetAnalogAverageBits(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return averageBits;
}
@@ -104,14 +104,14 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogOversampleBits(m_port, bits, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int AnalogInput::GetOversampleBits() const {
if (StatusIsFatal()) return 0;
int32_t status = 0;
int oversampleBits = HAL_GetAnalogOversampleBits(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return oversampleBits;
}
@@ -119,7 +119,7 @@
if (StatusIsFatal()) return 0;
int32_t status = 0;
int lsbWeight = HAL_GetAnalogLSBWeight(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return lsbWeight;
}
@@ -127,7 +127,7 @@
if (StatusIsFatal()) return 0;
int32_t status = 0;
int offset = HAL_GetAnalogOffset(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return offset;
}
@@ -135,7 +135,7 @@
if (StatusIsFatal()) return false;
int32_t status = 0;
bool isAccum = HAL_IsAccumulatorChannel(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return isAccum;
}
@@ -144,7 +144,7 @@
m_accumulatorOffset = 0;
int32_t status = 0;
HAL_InitAccumulator(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void AnalogInput::SetAccumulatorInitialValue(int64_t initialValue) {
@@ -156,7 +156,7 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_ResetAccumulator(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
if (!StatusIsFatal()) {
// Wait until the next sample, so the next call to GetAccumulator*()
@@ -172,21 +172,21 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAccumulatorCenter(m_port, center, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void AnalogInput::SetAccumulatorDeadband(int deadband) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAccumulatorDeadband(m_port, deadband, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int64_t AnalogInput::GetAccumulatorValue() const {
if (StatusIsFatal()) return 0;
int32_t status = 0;
int64_t value = HAL_GetAccumulatorValue(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value + m_accumulatorOffset;
}
@@ -194,7 +194,7 @@
if (StatusIsFatal()) return 0;
int32_t status = 0;
int64_t count = HAL_GetAccumulatorCount(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return count;
}
@@ -202,20 +202,20 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_GetAccumulatorOutput(m_port, &value, &count, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
value += m_accumulatorOffset;
}
void AnalogInput::SetSampleRate(double samplesPerSecond) {
int32_t status = 0;
HAL_SetAnalogSampleRate(samplesPerSecond, &status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
}
double AnalogInput::GetSampleRate() {
int32_t status = 0;
double sampleRate = HAL_GetAnalogSampleRate(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return sampleRate;
}
diff --git a/wpilibc/src/main/native/cpp/AnalogOutput.cpp b/wpilibc/src/main/native/cpp/AnalogOutput.cpp
index 5e56efc..041b446 100644
--- a/wpilibc/src/main/native/cpp/AnalogOutput.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogOutput.cpp
@@ -10,7 +10,9 @@
#include <limits>
#include <utility>
-#include <hal/HAL.h>
+#include <hal/AnalogOutput.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/Ports.h>
#include "frc/SensorUtil.h"
@@ -35,14 +37,13 @@
int32_t status = 0;
m_port = HAL_InitializeAnalogOutputPort(port, &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumAnalogOutputs(), channel,
- HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumAnalogOutputs(), channel);
m_channel = std::numeric_limits<int>::max();
m_port = HAL_kInvalidHandle;
return;
}
- HAL_Report(HALUsageReporting::kResourceType_AnalogOutput, m_channel);
+ HAL_Report(HALUsageReporting::kResourceType_AnalogOutput, m_channel + 1);
SendableRegistry::GetInstance().AddLW(this, "AnalogOutput", m_channel);
}
@@ -52,14 +53,14 @@
int32_t status = 0;
HAL_SetAnalogOutput(m_port, voltage, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
double AnalogOutput::GetVoltage() const {
int32_t status = 0;
double voltage = HAL_GetAnalogOutput(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return voltage;
}
diff --git a/wpilibc/src/main/native/cpp/AnalogTrigger.cpp b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
index 60ce04a..ddbb7c8 100644
--- a/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
@@ -9,9 +9,10 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/AnalogInput.h"
+#include "frc/DutyCycle.h"
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableRegistry.h"
@@ -26,19 +27,31 @@
AnalogTrigger::AnalogTrigger(AnalogInput* input) {
m_analogInput = input;
int32_t status = 0;
- int index = 0;
- m_trigger = HAL_InitializeAnalogTrigger(input->m_port, &index, &status);
+ m_trigger = HAL_InitializeAnalogTrigger(input->m_port, &status);
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
- m_index = std::numeric_limits<int>::max();
+ wpi_setHALError(status);
m_trigger = HAL_kInvalidHandle;
return;
}
- m_index = index;
+ int index = GetIndex();
- HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, input->m_channel);
- SendableRegistry::GetInstance().AddLW(this, "AnalogTrigger",
- input->GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, index + 1);
+ SendableRegistry::GetInstance().AddLW(this, "AnalogTrigger", index);
+}
+
+AnalogTrigger::AnalogTrigger(DutyCycle* input) {
+ m_dutyCycle = input;
+ int32_t status = 0;
+ m_trigger = HAL_InitializeAnalogTriggerDutyCycle(input->m_handle, &status);
+ if (status != 0) {
+ wpi_setHALError(status);
+ m_trigger = HAL_kInvalidHandle;
+ return;
+ }
+ int index = GetIndex();
+
+ HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, index + 1);
+ SendableRegistry::GetInstance().AddLW(this, "AnalogTrigger", index);
}
AnalogTrigger::~AnalogTrigger() {
@@ -53,9 +66,9 @@
AnalogTrigger::AnalogTrigger(AnalogTrigger&& rhs)
: ErrorBase(std::move(rhs)),
SendableHelper(std::move(rhs)),
- m_index(std::move(rhs.m_index)),
m_trigger(std::move(rhs.m_trigger)) {
std::swap(m_analogInput, rhs.m_analogInput);
+ std::swap(m_dutyCycle, rhs.m_dutyCycle);
std::swap(m_ownsAnalog, rhs.m_ownsAnalog);
}
@@ -63,9 +76,9 @@
ErrorBase::operator=(std::move(rhs));
SendableHelper::operator=(std::move(rhs));
- m_index = std::move(rhs.m_index);
m_trigger = std::move(rhs.m_trigger);
std::swap(m_analogInput, rhs.m_analogInput);
+ std::swap(m_dutyCycle, rhs.m_dutyCycle);
std::swap(m_ownsAnalog, rhs.m_ownsAnalog);
return *this;
@@ -75,40 +88,50 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogTriggerLimitsVoltage(m_trigger, lower, upper, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
+}
+
+void AnalogTrigger::SetLimitsDutyCycle(double lower, double upper) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAnalogTriggerLimitsDutyCycle(m_trigger, lower, upper, &status);
+ wpi_setHALError(status);
}
void AnalogTrigger::SetLimitsRaw(int lower, int upper) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogTriggerLimitsRaw(m_trigger, lower, upper, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void AnalogTrigger::SetAveraged(bool useAveragedValue) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogTriggerAveraged(m_trigger, useAveragedValue, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void AnalogTrigger::SetFiltered(bool useFilteredValue) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogTriggerFiltered(m_trigger, useFilteredValue, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int AnalogTrigger::GetIndex() const {
if (StatusIsFatal()) return -1;
- return m_index;
+ int32_t status = 0;
+ auto ret = HAL_GetAnalogTriggerFPGAIndex(m_trigger, &status);
+ wpi_setHALError(status);
+ return ret;
}
bool AnalogTrigger::GetInWindow() {
if (StatusIsFatal()) return false;
int32_t status = 0;
bool result = HAL_GetAnalogTriggerInWindow(m_trigger, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return result;
}
@@ -116,7 +139,7 @@
if (StatusIsFatal()) return false;
int32_t status = 0;
bool result = HAL_GetAnalogTriggerTriggerState(m_trigger, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return result;
}
diff --git a/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp b/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
index c2c7265..8fba479 100644
--- a/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
@@ -7,7 +7,7 @@
#include "frc/AnalogTriggerOutput.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/AnalogTrigger.h"
#include "frc/WPIErrors.h"
@@ -19,7 +19,7 @@
bool result = HAL_GetAnalogTriggerOutput(
m_trigger->m_trigger, static_cast<HAL_AnalogTriggerType>(m_outputType),
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return result;
}
@@ -33,7 +33,7 @@
bool AnalogTriggerOutput::IsAnalogTrigger() const { return true; }
-int AnalogTriggerOutput::GetChannel() const { return m_trigger->m_index; }
+int AnalogTriggerOutput::GetChannel() const { return m_trigger->GetIndex(); }
void AnalogTriggerOutput::InitSendable(SendableBuilder&) {}
@@ -41,5 +41,5 @@
AnalogTriggerType outputType)
: m_trigger(&trigger), m_outputType(outputType) {
HAL_Report(HALUsageReporting::kResourceType_AnalogTriggerOutput,
- trigger.GetIndex(), static_cast<uint8_t>(outputType));
+ trigger.GetIndex() + 1, static_cast<uint8_t>(outputType) + 1);
}
diff --git a/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
index 71c8e89..d7cdef6 100644
--- a/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
+++ b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
@@ -8,7 +8,7 @@
#include "frc/BuiltInAccelerometer.h"
#include <hal/Accelerometer.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
diff --git a/wpilibc/src/main/native/cpp/CAN.cpp b/wpilibc/src/main/native/cpp/CAN.cpp
index 35a926a..797b9ff 100644
--- a/wpilibc/src/main/native/cpp/CAN.cpp
+++ b/wpilibc/src/main/native/cpp/CAN.cpp
@@ -13,7 +13,6 @@
#include <hal/CANAPI.h>
#include <hal/Errors.h>
#include <hal/FRCUsageReporting.h>
-#include <hal/HALBase.h>
using namespace frc;
@@ -22,12 +21,12 @@
m_handle =
HAL_InitializeCAN(kTeamManufacturer, deviceId, kTeamDeviceType, &status);
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_handle = HAL_kInvalidHandle;
return;
}
- HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId);
+ HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId + 1);
}
CAN::CAN(int deviceId, int deviceManufacturer, int deviceType) {
@@ -36,12 +35,12 @@
static_cast<HAL_CANManufacturer>(deviceManufacturer), deviceId,
static_cast<HAL_CANDeviceType>(deviceType), &status);
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_handle = HAL_kInvalidHandle;
return;
}
- HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId);
+ HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId + 1);
}
CAN::~CAN() {
@@ -55,26 +54,26 @@
void CAN::WritePacket(const uint8_t* data, int length, int apiId) {
int32_t status = 0;
HAL_WriteCANPacket(m_handle, data, length, apiId, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void CAN::WritePacketRepeating(const uint8_t* data, int length, int apiId,
int repeatMs) {
int32_t status = 0;
HAL_WriteCANPacketRepeating(m_handle, data, length, apiId, repeatMs, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void CAN::WriteRTRFrame(int length, int apiId) {
int32_t status = 0;
HAL_WriteCANRTRFrame(m_handle, length, apiId, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void CAN::StopPacketRepeating(int apiId) {
int32_t status = 0;
HAL_StopCANPacketRepeating(m_handle, apiId, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
bool CAN::ReadPacketNew(int apiId, CANData* data) {
@@ -85,7 +84,7 @@
return false;
}
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return false;
} else {
return true;
@@ -100,7 +99,7 @@
return false;
}
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return false;
} else {
return true;
@@ -116,7 +115,7 @@
return false;
}
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return false;
} else {
return true;
diff --git a/wpilibc/src/main/native/cpp/Compressor.cpp b/wpilibc/src/main/native/cpp/Compressor.cpp
index 10d75f1..29789be 100644
--- a/wpilibc/src/main/native/cpp/Compressor.cpp
+++ b/wpilibc/src/main/native/cpp/Compressor.cpp
@@ -8,7 +8,7 @@
#include "frc/Compressor.h"
#include <hal/Compressor.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/Ports.h>
#include <hal/Solenoid.h>
@@ -22,13 +22,12 @@
int32_t status = 0;
m_compressorHandle = HAL_InitializeCompressor(m_module, &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumPCMModules(), pcmID,
- HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumPCMModules(), pcmID);
return;
}
SetClosedLoopControl(true);
- HAL_Report(HALUsageReporting::kResourceType_Compressor, pcmID);
+ HAL_Report(HALUsageReporting::kResourceType_Compressor, pcmID + 1);
SendableRegistry::GetInstance().AddLW(this, "Compressor", pcmID);
}
diff --git a/wpilibc/src/main/native/cpp/Counter.cpp b/wpilibc/src/main/native/cpp/Counter.cpp
index 1ef40fd..d2158ca 100644
--- a/wpilibc/src/main/native/cpp/Counter.cpp
+++ b/wpilibc/src/main/native/cpp/Counter.cpp
@@ -9,7 +9,8 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/Counter.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/AnalogTrigger.h"
#include "frc/DigitalInput.h"
@@ -22,11 +23,11 @@
Counter::Counter(Mode mode) {
int32_t status = 0;
m_counter = HAL_InitializeCounter((HAL_Counter_Mode)mode, &m_index, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
- SetMaxPeriod(.5);
+ SetMaxPeriod(0.5);
- HAL_Report(HALUsageReporting::kResourceType_Counter, m_index, mode);
+ HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1, mode + 1);
SendableRegistry::GetInstance().AddLW(this, "Counter", m_index);
}
@@ -81,7 +82,7 @@
HAL_SetCounterAverageSize(m_counter, 2, &status);
}
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
SetDownSourceEdge(inverted, true);
}
@@ -90,7 +91,7 @@
int32_t status = 0;
HAL_FreeCounter(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetUpSource(int channel) {
@@ -128,7 +129,7 @@
m_counter, source->GetPortHandleForRouting(),
(HAL_AnalogTriggerType)source->GetAnalogTriggerTypeForRouting(),
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
}
@@ -146,7 +147,7 @@
}
int32_t status = 0;
HAL_SetCounterUpSourceEdge(m_counter, risingEdge, fallingEdge, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::ClearUpSource() {
@@ -154,7 +155,7 @@
m_upSource.reset();
int32_t status = 0;
HAL_ClearCounterUpSource(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetDownSource(int channel) {
@@ -197,7 +198,7 @@
m_counter, source->GetPortHandleForRouting(),
(HAL_AnalogTriggerType)source->GetAnalogTriggerTypeForRouting(),
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
}
@@ -210,7 +211,7 @@
}
int32_t status = 0;
HAL_SetCounterDownSourceEdge(m_counter, risingEdge, fallingEdge, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::ClearDownSource() {
@@ -218,42 +219,42 @@
m_downSource.reset();
int32_t status = 0;
HAL_ClearCounterDownSource(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetUpDownCounterMode() {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterUpDownMode(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetExternalDirectionMode() {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterExternalDirectionMode(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetSemiPeriodMode(bool highSemiPeriod) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterSemiPeriodMode(m_counter, highSemiPeriod, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetPulseLengthMode(double threshold) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterPulseLengthMode(m_counter, threshold, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetReverseDirection(bool reverseDirection) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterReverseDirection(m_counter, reverseDirection, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetSamplesToAverage(int samplesToAverage) {
@@ -264,13 +265,13 @@
}
int32_t status = 0;
HAL_SetCounterSamplesToAverage(m_counter, samplesToAverage, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int Counter::GetSamplesToAverage() const {
int32_t status = 0;
int samples = HAL_GetCounterSamplesToAverage(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return samples;
}
@@ -280,7 +281,7 @@
if (StatusIsFatal()) return 0;
int32_t status = 0;
int value = HAL_GetCounter(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -288,14 +289,14 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_ResetCounter(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
double Counter::GetPeriod() const {
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double value = HAL_GetCounterPeriod(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -303,21 +304,21 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterMaxPeriod(m_counter, maxPeriod, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetUpdateWhenEmpty(bool enabled) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterUpdateWhenEmpty(m_counter, enabled, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
bool Counter::GetStopped() const {
if (StatusIsFatal()) return false;
int32_t status = 0;
bool value = HAL_GetCounterStopped(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -325,7 +326,7 @@
if (StatusIsFatal()) return false;
int32_t status = 0;
bool value = HAL_GetCounterDirection(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
diff --git a/wpilibc/src/main/native/cpp/DMA.cpp b/wpilibc/src/main/native/cpp/DMA.cpp
new file mode 100644
index 0000000..1861555
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DMA.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/DMA.h"
+
+#include <frc/AnalogInput.h>
+#include <frc/Counter.h>
+#include <frc/DigitalSource.h>
+#include <frc/DutyCycle.h>
+#include <frc/Encoder.h>
+
+#include <hal/DMA.h>
+#include <hal/HALBase.h>
+
+using namespace frc;
+
+DMA::DMA() {
+ int32_t status = 0;
+ dmaHandle = HAL_InitializeDMA(&status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+DMA::~DMA() { HAL_FreeDMA(dmaHandle); }
+
+void DMA::SetPause(bool pause) {
+ int32_t status = 0;
+ HAL_SetDMAPause(dmaHandle, pause, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::SetRate(int cycles) {
+ int32_t status = 0;
+ HAL_SetDMARate(dmaHandle, cycles, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddEncoder(const Encoder* encoder) {
+ int32_t status = 0;
+ HAL_AddDMAEncoder(dmaHandle, encoder->m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddEncoderPeriod(const Encoder* encoder) {
+ int32_t status = 0;
+ HAL_AddDMAEncoderPeriod(dmaHandle, encoder->m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddCounter(const Counter* counter) {
+ int32_t status = 0;
+ HAL_AddDMACounter(dmaHandle, counter->m_counter, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddCounterPeriod(const Counter* counter) {
+ int32_t status = 0;
+ HAL_AddDMACounterPeriod(dmaHandle, counter->m_counter, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddDigitalSource(const DigitalSource* digitalSource) {
+ int32_t status = 0;
+ HAL_AddDMADigitalSource(dmaHandle, digitalSource->GetPortHandleForRouting(),
+ &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddDutyCycle(const DutyCycle* dutyCycle) {
+ int32_t status = 0;
+ HAL_AddDMADutyCycle(dmaHandle, dutyCycle->m_handle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddAnalogInput(const AnalogInput* analogInput) {
+ int32_t status = 0;
+ HAL_AddDMAAnalogInput(dmaHandle, analogInput->m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddAveragedAnalogInput(const AnalogInput* analogInput) {
+ int32_t status = 0;
+ HAL_AddDMAAveragedAnalogInput(dmaHandle, analogInput->m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddAnalogAccumulator(const AnalogInput* analogInput) {
+ int32_t status = 0;
+ HAL_AddDMAAnalogAccumulator(dmaHandle, analogInput->m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::SetExternalTrigger(DigitalSource* source, bool rising, bool falling) {
+ int32_t status = 0;
+ HAL_SetDMAExternalTrigger(dmaHandle, source->GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(
+ source->GetAnalogTriggerTypeForRouting()),
+ rising, falling, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::StartDMA(int queueDepth) {
+ int32_t status = 0;
+ HAL_StartDMA(dmaHandle, queueDepth, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::StopDMA() {
+ int32_t status = 0;
+ HAL_StopDMA(dmaHandle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
diff --git a/wpilibc/src/main/native/cpp/DMC60.cpp b/wpilibc/src/main/native/cpp/DMC60.cpp
index d61bd40..e7d2c65 100644
--- a/wpilibc/src/main/native/cpp/DMC60.cpp
+++ b/wpilibc/src/main/native/cpp/DMC60.cpp
@@ -7,32 +7,18 @@
#include "frc/DMC60.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
DMC60::DMC60(int channel) : PWMSpeedController(channel) {
- /*
- * Note that the DMC 60 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.
- *
- * 2.004ms = full "forward"
- * 1.52ms = the "high end" of the deadband range
- * 1.50ms = center of the deadband range (off)
- * 1.48ms = the "low end" of the deadband range
- * 0.997ms = full "reverse"
- */
- SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_DigilentDMC60, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_DigilentDMC60, GetChannel() + 1);
SendableRegistry::GetInstance().SetName(this, "DMC60", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp b/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp
index 9e92da8..3cec3f1 100644
--- a/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp
@@ -13,7 +13,7 @@
#include <hal/Constants.h>
#include <hal/DIO.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/Counter.h"
#include "frc/Encoder.h"
@@ -38,7 +38,7 @@
*index = true;
HAL_Report(HALUsageReporting::kResourceType_DigitalGlitchFilter,
- m_channelIndex);
+ m_channelIndex + 1);
SendableRegistry::GetInstance().AddLW(this, "DigitalGlitchFilter",
m_channelIndex);
}
@@ -81,15 +81,12 @@
int32_t status = 0;
HAL_SetFilterSelect(input->GetPortHandleForRouting(), requestedIndex,
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
// Validate that we set it correctly.
int actualIndex =
HAL_GetFilterSelect(input->GetPortHandleForRouting(), &status);
wpi_assertEqual(actualIndex, requestedIndex);
-
- HAL_Report(HALUsageReporting::kResourceType_DigitalInput,
- input->GetChannel());
}
}
@@ -130,7 +127,7 @@
void DigitalGlitchFilter::SetPeriodCycles(int fpgaCycles) {
int32_t status = 0;
HAL_SetFilterPeriod(m_channelIndex, fpgaCycles, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void DigitalGlitchFilter::SetPeriodNanoSeconds(uint64_t nanoseconds) {
@@ -139,14 +136,14 @@
nanoseconds * HAL_GetSystemClockTicksPerMicrosecond() / 4 / 1000;
HAL_SetFilterPeriod(m_channelIndex, fpgaCycles, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int DigitalGlitchFilter::GetPeriodCycles() {
int32_t status = 0;
int fpgaCycles = HAL_GetFilterPeriod(m_channelIndex, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return fpgaCycles;
}
@@ -155,7 +152,7 @@
int32_t status = 0;
int fpgaCycles = HAL_GetFilterPeriod(m_channelIndex, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return static_cast<uint64_t>(fpgaCycles) * 1000L /
static_cast<uint64_t>(HAL_GetSystemClockTicksPerMicrosecond() / 4);
diff --git a/wpilibc/src/main/native/cpp/DigitalInput.cpp b/wpilibc/src/main/native/cpp/DigitalInput.cpp
index d0fa41b..7210750 100644
--- a/wpilibc/src/main/native/cpp/DigitalInput.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalInput.cpp
@@ -11,7 +11,8 @@
#include <utility>
#include <hal/DIO.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/Ports.h>
#include "frc/SensorUtil.h"
@@ -33,14 +34,13 @@
int32_t status = 0;
m_handle = HAL_InitializeDIOPort(HAL_GetPort(channel), true, &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumDigitalChannels(),
- channel, HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumDigitalChannels(), channel);
m_handle = HAL_kInvalidHandle;
m_channel = std::numeric_limits<int>::max();
return;
}
- HAL_Report(HALUsageReporting::kResourceType_DigitalInput, channel);
+ HAL_Report(HALUsageReporting::kResourceType_DigitalInput, channel + 1);
SendableRegistry::GetInstance().AddLW(this, "DigitalInput", channel);
}
@@ -53,7 +53,7 @@
if (StatusIsFatal()) return false;
int32_t status = 0;
bool value = HAL_GetDIO(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
diff --git a/wpilibc/src/main/native/cpp/DigitalOutput.cpp b/wpilibc/src/main/native/cpp/DigitalOutput.cpp
index 2068b5d..6e9b09b 100644
--- a/wpilibc/src/main/native/cpp/DigitalOutput.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalOutput.cpp
@@ -11,7 +11,8 @@
#include <utility>
#include <hal/DIO.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/Ports.h>
#include "frc/SensorUtil.h"
@@ -34,14 +35,13 @@
int32_t status = 0;
m_handle = HAL_InitializeDIOPort(HAL_GetPort(channel), false, &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumDigitalChannels(),
- channel, HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumDigitalChannels(), channel);
m_channel = std::numeric_limits<int>::max();
m_handle = HAL_kInvalidHandle;
return;
}
- HAL_Report(HALUsageReporting::kResourceType_DigitalOutput, channel);
+ HAL_Report(HALUsageReporting::kResourceType_DigitalOutput, channel + 1);
SendableRegistry::GetInstance().AddLW(this, "DigitalOutput", channel);
}
@@ -58,7 +58,7 @@
int32_t status = 0;
HAL_SetDIO(m_handle, value, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
bool DigitalOutput::Get() const {
@@ -66,10 +66,18 @@
int32_t status = 0;
bool val = HAL_GetDIO(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return val;
}
+HAL_Handle DigitalOutput::GetPortHandleForRouting() const { return m_handle; }
+
+AnalogTriggerType DigitalOutput::GetAnalogTriggerTypeForRouting() const {
+ return (AnalogTriggerType)0;
+}
+
+bool DigitalOutput::IsAnalogTrigger() const { return false; }
+
int DigitalOutput::GetChannel() const { return m_channel; }
void DigitalOutput::Pulse(double length) {
@@ -77,7 +85,7 @@
int32_t status = 0;
HAL_Pulse(m_handle, length, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
bool DigitalOutput::IsPulsing() const {
@@ -85,7 +93,7 @@
int32_t status = 0;
bool value = HAL_IsPulsing(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -94,7 +102,7 @@
int32_t status = 0;
HAL_SetDigitalPWMRate(rate, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void DigitalOutput::EnablePWM(double initialDutyCycle) {
@@ -104,15 +112,15 @@
if (StatusIsFatal()) return;
m_pwmGenerator = HAL_AllocateDigitalPWM(&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
if (StatusIsFatal()) return;
HAL_SetDigitalPWMDutyCycle(m_pwmGenerator, initialDutyCycle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
if (StatusIsFatal()) return;
HAL_SetDigitalPWMOutputChannel(m_pwmGenerator, m_channel, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void DigitalOutput::DisablePWM() {
@@ -124,10 +132,10 @@
// Disable the output by routing to a dead bit.
HAL_SetDigitalPWMOutputChannel(m_pwmGenerator, SensorUtil::kDigitalChannels,
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_FreeDigitalPWM(m_pwmGenerator, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_pwmGenerator = HAL_kInvalidHandle;
}
@@ -137,7 +145,7 @@
int32_t status = 0;
HAL_SetDigitalPWMDutyCycle(m_pwmGenerator, dutyCycle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void DigitalOutput::SetSimDevice(HAL_SimDeviceHandle device) {
diff --git a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
index 0b35ff5..c896ea8 100644
--- a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
+++ b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
@@ -9,7 +9,8 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/Ports.h>
#include <hal/Solenoid.h>
@@ -50,8 +51,8 @@
m_forwardHandle = HAL_InitializeSolenoidPort(
HAL_GetPortWithModule(moduleNumber, m_forwardChannel), &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumSolenoidChannels(),
- forwardChannel, HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumSolenoidChannels(),
+ forwardChannel);
m_forwardHandle = HAL_kInvalidHandle;
m_reverseHandle = HAL_kInvalidHandle;
return;
@@ -60,8 +61,8 @@
m_reverseHandle = HAL_InitializeSolenoidPort(
HAL_GetPortWithModule(moduleNumber, m_reverseChannel), &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumSolenoidChannels(),
- reverseChannel, HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumSolenoidChannels(),
+ reverseChannel);
// free forward solenoid
HAL_FreeSolenoidPort(m_forwardHandle);
m_forwardHandle = HAL_kInvalidHandle;
@@ -72,10 +73,10 @@
m_forwardMask = 1 << m_forwardChannel;
m_reverseMask = 1 << m_reverseChannel;
- HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_forwardChannel,
- m_moduleNumber);
- HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_reverseChannel,
- m_moduleNumber);
+ HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_forwardChannel + 1,
+ m_moduleNumber + 1);
+ HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_reverseChannel + 1,
+ m_moduleNumber + 1);
SendableRegistry::GetInstance().AddLW(this, "DoubleSolenoid", m_moduleNumber,
m_forwardChannel);
@@ -110,8 +111,8 @@
int rstatus = 0;
HAL_SetSolenoid(m_reverseHandle, reverse, &rstatus);
- wpi_setErrorWithContext(fstatus, HAL_GetErrorMessage(fstatus));
- wpi_setErrorWithContext(rstatus, HAL_GetErrorMessage(rstatus));
+ wpi_setHALError(fstatus);
+ wpi_setHALError(rstatus);
}
DoubleSolenoid::Value DoubleSolenoid::Get() const {
@@ -121,8 +122,8 @@
bool valueForward = HAL_GetSolenoid(m_forwardHandle, &fstatus);
bool valueReverse = HAL_GetSolenoid(m_reverseHandle, &rstatus);
- wpi_setErrorWithContext(fstatus, HAL_GetErrorMessage(fstatus));
- wpi_setErrorWithContext(rstatus, HAL_GetErrorMessage(rstatus));
+ wpi_setHALError(fstatus);
+ wpi_setHALError(rstatus);
if (valueForward) return kForward;
if (valueReverse) return kReverse;
diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp
index d268de7..7c64883 100644
--- a/wpilibc/src/main/native/cpp/DriverStation.cpp
+++ b/wpilibc/src/main/native/cpp/DriverStation.cpp
@@ -9,7 +9,9 @@
#include <chrono>
-#include <hal/HAL.h>
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/Power.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableEntry.h>
@@ -473,6 +475,13 @@
return voltage;
}
+void DriverStation::WakeupWaitForData() {
+ std::scoped_lock waitLock(m_waitForDataMutex);
+ // Nofify all threads
+ m_waitForDataCounter++;
+ m_waitForDataCond.notify_all();
+}
+
void DriverStation::GetData() {
{
// Compute the pressed and released buttons
@@ -494,13 +503,7 @@
}
}
- {
- std::scoped_lock waitLock(m_waitForDataMutex);
- // Nofify all threads
- m_waitForDataCounter++;
- m_waitForDataCond.notify_all();
- }
-
+ WakeupWaitForData();
SendMatchData();
}
diff --git a/wpilibc/src/main/native/cpp/DutyCycle.cpp b/wpilibc/src/main/native/cpp/DutyCycle.cpp
new file mode 100644
index 0000000..12f390e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DutyCycle.cpp
@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/DutyCycle.h"
+
+#include <hal/DutyCycle.h>
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/DigitalSource.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+DutyCycle::DutyCycle(DigitalSource* source)
+ : m_source{source, NullDeleter<DigitalSource>()} {
+ if (m_source == nullptr) {
+ wpi_setWPIError(NullParameter);
+ } else {
+ InitDutyCycle();
+ }
+}
+
+DutyCycle::DutyCycle(DigitalSource& source)
+ : m_source{&source, NullDeleter<DigitalSource>()} {
+ InitDutyCycle();
+}
+
+DutyCycle::DutyCycle(std::shared_ptr<DigitalSource> source)
+ : m_source{std::move(source)} {
+ if (m_source == nullptr) {
+ wpi_setWPIError(NullParameter);
+ } else {
+ InitDutyCycle();
+ }
+}
+
+DutyCycle::~DutyCycle() { HAL_FreeDutyCycle(m_handle); }
+
+void DutyCycle::InitDutyCycle() {
+ int32_t status = 0;
+ m_handle =
+ HAL_InitializeDutyCycle(m_source->GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(
+ m_source->GetAnalogTriggerTypeForRouting()),
+ &status);
+ wpi_setHALError(status);
+ int index = GetFPGAIndex();
+ HAL_Report(HALUsageReporting::kResourceType_DutyCycle, index + 1);
+ SendableRegistry::GetInstance().AddLW(this, "Duty Cycle", index);
+}
+
+int DutyCycle::GetFPGAIndex() const {
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleFPGAIndex(m_handle, &status);
+ wpi_setHALError(status);
+ return retVal;
+}
+
+int DutyCycle::GetFrequency() const {
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleFrequency(m_handle, &status);
+ wpi_setHALError(status);
+ return retVal;
+}
+
+double DutyCycle::GetOutput() const {
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleOutput(m_handle, &status);
+ wpi_setHALError(status);
+ return retVal;
+}
+
+unsigned int DutyCycle::GetOutputRaw() const {
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleOutputRaw(m_handle, &status);
+ wpi_setHALError(status);
+ return retVal;
+}
+
+unsigned int DutyCycle::GetOutputScaleFactor() const {
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleOutputScaleFactor(m_handle, &status);
+ wpi_setHALError(status);
+ return retVal;
+}
+
+int DutyCycle::GetSourceChannel() const { return m_source->GetChannel(); }
+
+void DutyCycle::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Duty Cycle");
+ builder.AddDoubleProperty("Frequency",
+ [this] { return this->GetFrequency(); }, nullptr);
+ builder.AddDoubleProperty("Output", [this] { return this->GetOutput(); },
+ nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
new file mode 100644
index 0000000..728b1de
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
@@ -0,0 +1,150 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/DutyCycleEncoder.h"
+
+#include "frc/Counter.h"
+#include "frc/DigitalInput.h"
+#include "frc/DigitalSource.h"
+#include "frc/DriverStation.h"
+#include "frc/DutyCycle.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+DutyCycleEncoder::DutyCycleEncoder(int channel)
+ : m_dutyCycle{std::make_shared<DutyCycle>(
+ std::make_shared<DigitalInput>(channel))},
+ m_analogTrigger{m_dutyCycle.get()},
+ m_counter{} {}
+
+DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle)
+ : m_dutyCycle{&dutyCycle, NullDeleter<DutyCycle>{}},
+ m_analogTrigger{m_dutyCycle.get()},
+ m_counter{} {
+ Init();
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle)
+ : m_dutyCycle{dutyCycle, NullDeleter<DutyCycle>{}},
+ m_analogTrigger{m_dutyCycle.get()},
+ m_counter{} {
+ Init();
+}
+
+DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DutyCycle> dutyCycle)
+ : m_dutyCycle{std::move(dutyCycle)},
+ m_analogTrigger{m_dutyCycle.get()},
+ m_counter{} {
+ Init();
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DigitalSource& digitalSource)
+ : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)},
+ m_analogTrigger{m_dutyCycle.get()},
+ m_counter{} {
+ Init();
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DigitalSource* digitalSource)
+ : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)},
+ m_analogTrigger{m_dutyCycle.get()},
+ m_counter{} {
+ Init();
+}
+
+DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DigitalSource> digitalSource)
+ : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)},
+ m_analogTrigger{m_dutyCycle.get()},
+ m_counter{} {
+ Init();
+}
+
+void DutyCycleEncoder::Init() {
+ m_simDevice = hal::SimDevice{"DutyCycleEncoder", m_dutyCycle->GetFPGAIndex()};
+
+ if (m_simDevice) {
+ 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.CreateOutput(AnalogTriggerType::kRisingPulse));
+ m_counter.SetDownSource(
+ m_analogTrigger.CreateOutput(AnalogTriggerType::kFallingPulse));
+
+ SendableRegistry::GetInstance().AddLW(this, "DutyCycle Encoder",
+ m_dutyCycle->GetSourceChannel());
+}
+
+units::turn_t DutyCycleEncoder::Get() const {
+ if (m_simPosition) return units::turn_t{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++) {
+ auto counter = m_counter.Get();
+ auto pos = m_dutyCycle->GetOutput();
+ auto counter2 = m_counter.Get();
+ auto pos2 = m_dutyCycle->GetOutput();
+ if (counter == counter2 && pos == pos2) {
+ units::turn_t turns{counter + pos - m_positionOffset};
+ m_lastPosition = turns;
+ return turns;
+ }
+ }
+
+ frc::DriverStation::GetInstance().ReportWarning(
+ "Failed to read DutyCycle Encoder. Potential Speed Overrun. Returning "
+ "last value");
+ return m_lastPosition;
+}
+
+void DutyCycleEncoder::SetDistancePerRotation(double distancePerRotation) {
+ m_distancePerRotation = distancePerRotation;
+}
+
+double DutyCycleEncoder::GetDistancePerRotation() const {
+ return m_distancePerRotation;
+}
+
+double DutyCycleEncoder::GetDistance() const {
+ return Get().to<double>() * GetDistancePerRotation();
+}
+
+int DutyCycleEncoder::GetFrequency() const {
+ return m_dutyCycle->GetFrequency();
+}
+
+void DutyCycleEncoder::Reset() {
+ m_counter.Reset();
+ m_positionOffset = m_dutyCycle->GetOutput();
+}
+
+bool DutyCycleEncoder::IsConnected() const {
+ if (m_simIsConnected) return m_simIsConnected.Get();
+ return GetFrequency() > m_frequencyThreshold;
+}
+
+void DutyCycleEncoder::SetConnectedFrequencyThreshold(int frequency) {
+ if (frequency < 0) {
+ frequency = 0;
+ }
+ m_frequencyThreshold = frequency;
+}
+
+void DutyCycleEncoder::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("AbsoluteEncoder");
+ builder.AddDoubleProperty("Distance", [this] { return this->GetDistance(); },
+ nullptr);
+ builder.AddDoubleProperty("Distance Per Rotation",
+ [this] { return this->GetDistancePerRotation(); },
+ nullptr);
+ builder.AddDoubleProperty("Is Connected",
+ [this] { return this->IsConnected(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/Encoder.cpp b/wpilibc/src/main/native/cpp/Encoder.cpp
index fb043cc..963bcc6 100644
--- a/wpilibc/src/main/native/cpp/Encoder.cpp
+++ b/wpilibc/src/main/native/cpp/Encoder.cpp
@@ -11,7 +11,6 @@
#include <hal/Encoder.h>
#include <hal/FRCUsageReporting.h>
-#include <hal/HALBase.h>
#include "frc/DigitalInput.h"
#include "frc/WPIErrors.h"
@@ -60,14 +59,14 @@
Encoder::~Encoder() {
int32_t status = 0;
HAL_FreeEncoder(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int Encoder::Get() const {
if (StatusIsFatal()) return 0;
int32_t status = 0;
int value = HAL_GetEncoder(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -75,14 +74,14 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_ResetEncoder(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
double Encoder::GetPeriod() const {
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double value = HAL_GetEncoderPeriod(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -90,14 +89,14 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetEncoderMaxPeriod(m_encoder, maxPeriod, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
bool Encoder::GetStopped() const {
if (StatusIsFatal()) return true;
int32_t status = 0;
bool value = HAL_GetEncoderStopped(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -105,7 +104,7 @@
if (StatusIsFatal()) return false;
int32_t status = 0;
bool value = HAL_GetEncoderDirection(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -113,14 +112,14 @@
if (StatusIsFatal()) return 0;
int32_t status = 0;
int value = HAL_GetEncoderRaw(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
int Encoder::GetEncodingScale() const {
int32_t status = 0;
int val = HAL_GetEncoderEncodingScale(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return val;
}
@@ -128,7 +127,7 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double value = HAL_GetEncoderDistance(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -136,7 +135,7 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double value = HAL_GetEncoderRate(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -144,21 +143,21 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetEncoderMinRate(m_encoder, minRate, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Encoder::SetDistancePerPulse(double distancePerPulse) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetEncoderDistancePerPulse(m_encoder, distancePerPulse, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
double Encoder::GetDistancePerPulse() const {
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double distancePerPulse = HAL_GetEncoderDistancePerPulse(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return distancePerPulse;
}
@@ -166,7 +165,7 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetEncoderReverseDirection(m_encoder, reverseDirection, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Encoder::SetSamplesToAverage(int samplesToAverage) {
@@ -178,13 +177,13 @@
}
int32_t status = 0;
HAL_SetEncoderSamplesToAverage(m_encoder, samplesToAverage, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int Encoder::GetSamplesToAverage() const {
int32_t status = 0;
int result = HAL_GetEncoderSamplesToAverage(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return result;
}
@@ -214,7 +213,7 @@
m_encoder, source.GetPortHandleForRouting(),
(HAL_AnalogTriggerType)source.GetAnalogTriggerTypeForRouting(),
(HAL_EncoderIndexingType)type, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Encoder::SetSimDevice(HAL_SimDeviceHandle device) {
@@ -224,14 +223,14 @@
int Encoder::GetFPGAIndex() const {
int32_t status = 0;
int val = HAL_GetEncoderFPGAIndex(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return val;
}
void Encoder::InitSendable(SendableBuilder& builder) {
int32_t status = 0;
HAL_EncoderEncodingType type = HAL_GetEncoderEncodingType(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
if (type == HAL_EncoderEncodingType::HAL_Encoder_k4X)
builder.SetSmartDashboardType("Quadrature Encoder");
else
@@ -252,9 +251,9 @@
m_bSource->GetPortHandleForRouting(),
(HAL_AnalogTriggerType)m_bSource->GetAnalogTriggerTypeForRouting(),
reverseDirection, (HAL_EncoderEncodingType)encodingType, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
- HAL_Report(HALUsageReporting::kResourceType_Encoder, GetFPGAIndex(),
+ HAL_Report(HALUsageReporting::kResourceType_Encoder, GetFPGAIndex() + 1,
encodingType);
SendableRegistry::GetInstance().AddLW(this, "Encoder",
m_aSource->GetChannel());
@@ -264,6 +263,6 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double val = HAL_GetEncoderDecodingScaleFactor(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return val;
}
diff --git a/wpilibc/src/main/native/cpp/ErrorBase.cpp b/wpilibc/src/main/native/cpp/ErrorBase.cpp
index 8e70e61..d098c07 100644
--- a/wpilibc/src/main/native/cpp/ErrorBase.cpp
+++ b/wpilibc/src/main/native/cpp/ErrorBase.cpp
@@ -12,7 +12,8 @@
#include <cstring>
#include <set>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <wpi/Format.h>
#include <wpi/SmallString.h>
#include <wpi/raw_ostream.h>
diff --git a/wpilibc/src/main/native/cpp/GenericHID.cpp b/wpilibc/src/main/native/cpp/GenericHID.cpp
index 504d669..a7dcfc0 100644
--- a/wpilibc/src/main/native/cpp/GenericHID.cpp
+++ b/wpilibc/src/main/native/cpp/GenericHID.cpp
@@ -7,7 +7,8 @@
#include "frc/GenericHID.h"
-#include <hal/HAL.h>
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/DriverStation.h"
#include "frc/WPIErrors.h"
diff --git a/wpilibc/src/main/native/cpp/I2C.cpp b/wpilibc/src/main/native/cpp/I2C.cpp
index 44b71af..21d92f2 100644
--- a/wpilibc/src/main/native/cpp/I2C.cpp
+++ b/wpilibc/src/main/native/cpp/I2C.cpp
@@ -9,7 +9,7 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/I2C.h>
#include "frc/WPIErrors.h"
@@ -20,7 +20,7 @@
: m_port(static_cast<HAL_I2CPort>(port)), m_deviceAddress(deviceAddress) {
int32_t status = 0;
HAL_InitializeI2C(m_port, &status);
- // wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ // wpi_setHALError(status);
HAL_Report(HALUsageReporting::kResourceType_I2C, deviceAddress);
}
@@ -32,7 +32,7 @@
int32_t status = 0;
status = HAL_TransactionI2C(m_port, m_deviceAddress, dataToSend, sendSize,
dataReceived, receiveSize);
- // wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ // wpi_setHALError(status);
return status < 0;
}
diff --git a/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp b/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
index 36150d4..202c61d 100644
--- a/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
+++ b/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
@@ -7,7 +7,7 @@
#include "frc/InterruptableSensorBase.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/Utility.h"
#include "frc/WPIErrors.h"
@@ -36,7 +36,7 @@
&status);
SetUpSourceEdge(true, false);
HAL_AttachInterruptHandler(m_interrupt, handler, param, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void InterruptableSensorBase::RequestInterrupts(InterruptEventHandler handler) {
@@ -69,7 +69,7 @@
(*self)(res);
},
m_interruptHandler.get(), &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void InterruptableSensorBase::RequestInterrupts() {
@@ -84,7 +84,7 @@
m_interrupt, GetPortHandleForRouting(),
static_cast<HAL_AnalogTriggerType>(GetAnalogTriggerTypeForRouting()),
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
SetUpSourceEdge(true, false);
}
@@ -106,7 +106,7 @@
int result;
result = HAL_WaitForInterrupt(m_interrupt, timeout, ignorePrevious, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
// Rising edge result is the interrupt bit set in the byte 0xFF
// Falling edge result is the interrupt bit set in the byte 0xFF00
@@ -122,7 +122,7 @@
wpi_assert(m_interrupt != HAL_kInvalidHandle);
int32_t status = 0;
HAL_EnableInterrupts(m_interrupt, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void InterruptableSensorBase::DisableInterrupts() {
@@ -130,7 +130,7 @@
wpi_assert(m_interrupt != HAL_kInvalidHandle);
int32_t status = 0;
HAL_DisableInterrupts(m_interrupt, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
double InterruptableSensorBase::ReadRisingTimestamp() {
@@ -138,7 +138,7 @@
wpi_assert(m_interrupt != HAL_kInvalidHandle);
int32_t status = 0;
int64_t timestamp = HAL_ReadInterruptRisingTimestamp(m_interrupt, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return timestamp * 1e-6;
}
@@ -147,7 +147,7 @@
wpi_assert(m_interrupt != HAL_kInvalidHandle);
int32_t status = 0;
int64_t timestamp = HAL_ReadInterruptFallingTimestamp(m_interrupt, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return timestamp * 1e-6;
}
@@ -163,7 +163,7 @@
if (m_interrupt != HAL_kInvalidHandle) {
int32_t status = 0;
HAL_SetInterruptUpSourceEdge(m_interrupt, risingEdge, fallingEdge, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
}
@@ -172,5 +172,5 @@
// Expects the calling leaf class to allocate an interrupt index.
int32_t status = 0;
m_interrupt = HAL_InitializeInterrupts(watcher, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
diff --git a/wpilibc/src/main/native/cpp/IterativeRobot.cpp b/wpilibc/src/main/native/cpp/IterativeRobot.cpp
index 950b239..c8664a5 100644
--- a/wpilibc/src/main/native/cpp/IterativeRobot.cpp
+++ b/wpilibc/src/main/native/cpp/IterativeRobot.cpp
@@ -7,7 +7,8 @@
#include "frc/IterativeRobot.h"
-#include <hal/HAL.h>
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/DriverStation.h"
@@ -30,7 +31,13 @@
while (true) {
// Wait for driver station data so the loop doesn't hog the CPU
DriverStation::GetInstance().WaitForData();
+ if (m_exit) break;
LoopFunc();
}
}
+
+void IterativeRobot::EndCompetition() {
+ m_exit = true;
+ DriverStation::GetInstance().WakeupWaitForData();
+}
diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
index 90dc54b..2997234 100644
--- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
+++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
@@ -9,14 +9,14 @@
#include <cstdio>
-#include <hal/HAL.h>
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
#include <wpi/Format.h>
#include <wpi/SmallString.h>
#include <wpi/raw_ostream.h>
#include "frc/DriverStation.h"
#include "frc/Timer.h"
-#include "frc/commands/Scheduler.h"
#include "frc/livewindow/LiveWindow.h"
#include "frc/shuffleboard/Shuffleboard.h"
#include "frc/smartdashboard/SmartDashboard.h"
@@ -131,7 +131,6 @@
TeleopInit();
m_watchdog.AddEpoch("TeleopInit()");
m_lastMode = Mode::kTeleop;
- Scheduler::GetInstance()->SetEnabled(true);
}
HAL_ObserveUserProgramTeleop();
diff --git a/wpilibc/src/main/native/cpp/Jaguar.cpp b/wpilibc/src/main/native/cpp/Jaguar.cpp
index 487d489..c87e4d1 100644
--- a/wpilibc/src/main/native/cpp/Jaguar.cpp
+++ b/wpilibc/src/main/native/cpp/Jaguar.cpp
@@ -7,26 +7,18 @@
#include "frc/Jaguar.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
Jaguar::Jaguar(int channel) : PWMSpeedController(channel) {
- /* Input profile defined by Luminary Micro.
- *
- * Full reverse ranges from 0.671325ms to 0.6972211ms
- * Proportional reverse ranges from 0.6972211ms to 1.4482078ms
- * Neutral ranges from 1.4482078ms to 1.5517922ms
- * Proportional forward ranges from 1.5517922ms to 2.3027789ms
- * Full forward ranges from 2.3027789ms to 2.328675ms
- */
- SetBounds(2.31, 1.55, 1.507, 1.454, .697);
+ SetBounds(2.31, 1.55, 1.507, 1.454, 0.697);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_Jaguar, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_Jaguar, GetChannel() + 1);
SendableRegistry::GetInstance().SetName(this, "Jaguar", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/Joystick.cpp b/wpilibc/src/main/native/cpp/Joystick.cpp
index 34afd0a..8d464cf 100644
--- a/wpilibc/src/main/native/cpp/Joystick.cpp
+++ b/wpilibc/src/main/native/cpp/Joystick.cpp
@@ -9,7 +9,7 @@
#include <cmath>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <wpi/math>
#include "frc/DriverStation.h"
@@ -24,7 +24,7 @@
m_axes[Axis::kTwist] = kDefaultTwistChannel;
m_axes[Axis::kThrottle] = kDefaultThrottleChannel;
- HAL_Report(HALUsageReporting::kResourceType_Joystick, port);
+ HAL_Report(HALUsageReporting::kResourceType_Joystick, port + 1);
}
void Joystick::SetXChannel(int channel) { m_axes[Axis::kX] = channel; }
diff --git a/wpilibc/src/main/native/cpp/LinearFilter.cpp b/wpilibc/src/main/native/cpp/LinearFilter.cpp
deleted file mode 100644
index 95df127..0000000
--- a/wpilibc/src/main/native/cpp/LinearFilter.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/LinearFilter.h"
-
-#include <cassert>
-#include <cmath>
-
-#include <hal/HAL.h>
-
-using namespace frc;
-
-LinearFilter::LinearFilter(wpi::ArrayRef<double> ffGains,
- wpi::ArrayRef<double> fbGains)
- : m_inputs(ffGains.size()),
- m_outputs(fbGains.size()),
- m_inputGains(ffGains),
- m_outputGains(fbGains) {
- static int instances = 0;
- instances++;
- HAL_Report(HALUsageReporting::kResourceType_LinearFilter, instances);
-}
-
-LinearFilter LinearFilter::SinglePoleIIR(double timeConstant,
- units::second_t period) {
- double gain = std::exp(-period.to<double>() / timeConstant);
- return LinearFilter(1.0 - gain, -gain);
-}
-
-LinearFilter LinearFilter::HighPass(double timeConstant,
- units::second_t period) {
- double gain = std::exp(-period.to<double>() / timeConstant);
- return LinearFilter({gain, -gain}, {-gain});
-}
-
-LinearFilter LinearFilter::MovingAverage(int taps) {
- assert(taps > 0);
-
- std::vector<double> gains(taps, 1.0 / taps);
- return LinearFilter(gains, {});
-}
-
-void LinearFilter::Reset() {
- m_inputs.reset();
- m_outputs.reset();
-}
-
-double LinearFilter::Calculate(double input) {
- double retVal = 0.0;
-
- // Rotate the inputs
- m_inputs.push_front(input);
-
- // Calculate the new value
- for (size_t i = 0; i < m_inputGains.size(); i++) {
- retVal += m_inputs[i] * m_inputGains[i];
- }
- for (size_t i = 0; i < m_outputGains.size(); i++) {
- retVal -= m_outputs[i] * m_outputGains[i];
- }
-
- // Rotate the outputs
- m_outputs.push_front(retVal);
-
- return retVal;
-}
diff --git a/wpilibc/src/main/native/cpp/NidecBrushless.cpp b/wpilibc/src/main/native/cpp/NidecBrushless.cpp
index 90ce43e..5bce36e 100644
--- a/wpilibc/src/main/native/cpp/NidecBrushless.cpp
+++ b/wpilibc/src/main/native/cpp/NidecBrushless.cpp
@@ -7,7 +7,7 @@
#include "frc/NidecBrushless.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableBuilder.h"
#include "frc/smartdashboard/SendableRegistry.h"
@@ -26,7 +26,7 @@
m_dio.SetPWMRate(15625);
m_dio.EnablePWM(0.5);
- HAL_Report(HALUsageReporting::kResourceType_NidecBrushless, pwmChannel);
+ HAL_Report(HALUsageReporting::kResourceType_NidecBrushless, pwmChannel + 1);
registry.AddLW(this, "Nidec Brushless", pwmChannel);
}
diff --git a/wpilibc/src/main/native/cpp/Notifier.cpp b/wpilibc/src/main/native/cpp/Notifier.cpp
index a2f93f9..a7fa038 100644
--- a/wpilibc/src/main/native/cpp/Notifier.cpp
+++ b/wpilibc/src/main/native/cpp/Notifier.cpp
@@ -9,7 +9,9 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/Notifier.h>
+#include <wpi/SmallString.h>
#include "frc/Timer.h"
#include "frc/Utility.h"
@@ -23,7 +25,7 @@
m_handler = handler;
int32_t status = 0;
m_notifier = HAL_InitializeNotifier(&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_thread = std::thread([=] {
for (;;) {
@@ -57,7 +59,7 @@
// atomically set handle to 0, then clean
HAL_NotifierHandle handle = m_notifier.exchange(0);
HAL_StopNotifier(handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
// Join the thread to ensure the handler has exited.
if (m_thread.joinable()) m_thread.join();
@@ -90,6 +92,13 @@
return *this;
}
+void Notifier::SetName(const wpi::Twine& name) {
+ wpi::SmallString<64> nameBuf;
+ int32_t status = 0;
+ HAL_SetNotifierName(m_notifier,
+ name.toNullTerminatedStringRef(nameBuf).data(), &status);
+}
+
void Notifier::SetHandler(std::function<void()> handler) {
std::scoped_lock lock(m_processMutex);
m_handler = handler;
@@ -122,7 +131,7 @@
void Notifier::Stop() {
int32_t status = 0;
HAL_CancelNotifierAlarm(m_notifier, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Notifier::UpdateAlarm(uint64_t triggerTime) {
@@ -131,7 +140,7 @@
auto notifier = m_notifier.load();
if (notifier == 0) return;
HAL_UpdateNotifierAlarm(notifier, triggerTime, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Notifier::UpdateAlarm() {
diff --git a/wpilibc/src/main/native/cpp/PIDBase.cpp b/wpilibc/src/main/native/cpp/PIDBase.cpp
index a2efe0f..c8f6fed 100644
--- a/wpilibc/src/main/native/cpp/PIDBase.cpp
+++ b/wpilibc/src/main/native/cpp/PIDBase.cpp
@@ -10,7 +10,7 @@
#include <algorithm>
#include <cmath>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/PIDOutput.h"
#include "frc/smartdashboard/SendableBuilder.h"
@@ -35,7 +35,7 @@
m_F = Kf;
m_pidInput = &source;
- m_filter = LinearFilter::MovingAverage(1);
+ m_filter = LinearFilter<double>::MovingAverage(1);
m_pidOutput = &output;
@@ -196,7 +196,7 @@
void PIDBase::SetToleranceBuffer(int bufLength) {
std::scoped_lock lock(m_thisMutex);
- m_filter = LinearFilter::MovingAverage(bufLength);
+ m_filter = LinearFilter<double>::MovingAverage(bufLength);
}
bool PIDBase::OnTarget() const {
diff --git a/wpilibc/src/main/native/cpp/PWM.cpp b/wpilibc/src/main/native/cpp/PWM.cpp
index c6b398f..dabcd2e 100644
--- a/wpilibc/src/main/native/cpp/PWM.cpp
+++ b/wpilibc/src/main/native/cpp/PWM.cpp
@@ -9,7 +9,8 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/PWM.h>
#include <hal/Ports.h>
@@ -31,8 +32,7 @@
int32_t status = 0;
m_handle = HAL_InitializePWMPort(HAL_GetPort(channel), &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumPWMChannels(), channel,
- HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumPWMChannels(), channel);
m_channel = std::numeric_limits<int>::max();
m_handle = HAL_kInvalidHandle;
return;
@@ -41,12 +41,12 @@
m_channel = channel;
HAL_SetPWMDisabled(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
status = 0;
HAL_SetPWMEliminateDeadband(m_handle, false, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
- HAL_Report(HALUsageReporting::kResourceType_PWM, channel);
+ HAL_Report(HALUsageReporting::kResourceType_PWM, channel + 1);
SendableRegistry::GetInstance().AddLW(this, "PWM", channel);
SetSafetyEnabled(false);
@@ -56,10 +56,10 @@
int32_t status = 0;
HAL_SetPWMDisabled(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_FreePWMPort(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void PWM::StopMotor() { SetDisabled(); }
@@ -73,7 +73,7 @@
int32_t status = 0;
HAL_SetPWMRaw(m_handle, value, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
uint16_t PWM::GetRaw() const {
@@ -81,7 +81,7 @@
int32_t status = 0;
uint16_t value = HAL_GetPWMRaw(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -90,14 +90,14 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetPWMPosition(m_handle, pos, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
double PWM::GetPosition() const {
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double position = HAL_GetPWMPosition(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return position;
}
@@ -105,7 +105,7 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetPWMSpeed(m_handle, speed, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
Feed();
}
@@ -114,7 +114,7 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double speed = HAL_GetPWMSpeed(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return speed;
}
@@ -124,7 +124,7 @@
int32_t status = 0;
HAL_SetPWMDisabled(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void PWM::SetPeriodMultiplier(PeriodMultiplier mult) {
@@ -148,7 +148,7 @@
wpi_assert(false);
}
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void PWM::SetZeroLatch() {
@@ -157,14 +157,14 @@
int32_t status = 0;
HAL_LatchPWMZero(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void PWM::EnableDeadbandElimination(bool eliminateDeadband) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetPWMEliminateDeadband(m_handle, eliminateDeadband, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void PWM::SetBounds(double max, double deadbandMax, double center,
@@ -173,7 +173,7 @@
int32_t status = 0;
HAL_SetPWMConfig(m_handle, max, deadbandMax, center, deadbandMin, min,
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void PWM::SetRawBounds(int max, int deadbandMax, int center, int deadbandMin,
@@ -182,7 +182,7 @@
int32_t status = 0;
HAL_SetPWMConfigRaw(m_handle, max, deadbandMax, center, deadbandMin, min,
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void PWM::GetRawBounds(int* max, int* deadbandMax, int* center,
@@ -190,7 +190,7 @@
int32_t status = 0;
HAL_GetPWMConfigRaw(m_handle, max, deadbandMax, center, deadbandMin, min,
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int PWM::GetChannel() const { return m_channel; }
diff --git a/wpilibc/src/main/native/cpp/PWMSparkMax.cpp b/wpilibc/src/main/native/cpp/PWMSparkMax.cpp
index d8d3b2a..c5375f4 100644
--- a/wpilibc/src/main/native/cpp/PWMSparkMax.cpp
+++ b/wpilibc/src/main/native/cpp/PWMSparkMax.cpp
@@ -7,26 +7,18 @@
#include "frc/PWMSparkMax.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
PWMSparkMax::PWMSparkMax(int channel) : PWMSpeedController(channel) {
- /* Note that the SparkMax uses the following bounds for PWM values.
- *
- * 2.003ms = full "forward"
- * 1.55ms = the "high end" of the deadband range
- * 1.50ms = center of the deadband range (off)
- * 1.46ms = the "low end" of the deadband range
- * 0.999ms = full "reverse"
- */
- SetBounds(2.003, 1.55, 1.50, 1.46, .999);
+ SetBounds(2.003, 1.55, 1.50, 1.46, 0.999);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_RevSparkMaxPWM, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_RevSparkMaxPWM, GetChannel() + 1);
SendableRegistry::GetInstance().SetName(this, "PWMSparkMax", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp b/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
index 0bbc5b9..1242be9 100644
--- a/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
+++ b/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
@@ -7,30 +7,18 @@
#include "frc/PWMTalonSRX.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
PWMTalonSRX::PWMTalonSRX(int channel) : PWMSpeedController(channel) {
- /* Note that the PWMTalonSRX 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 Cross The Road Electronics.
- * 2.004ms = full "forward"
- * 1.52ms = the "high end" of the deadband range
- * 1.50ms = center of the deadband range (off)
- * 1.48ms = the "low end" of the deadband range
- * 0.997ms = full "reverse"
- */
- SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_PWMTalonSRX, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_PWMTalonSRX, GetChannel() + 1);
SendableRegistry::GetInstance().SetName(this, "PWMTalonSRX", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp b/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp
index 122d219..0d966ae 100644
--- a/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp
+++ b/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp
@@ -7,30 +7,18 @@
#include "frc/PWMVictorSPX.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
PWMVictorSPX::PWMVictorSPX(int channel) : PWMSpeedController(channel) {
- /* Note that the PWMVictorSPX 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 VictorSPX User
- * Manual available from Cross The Road Electronics.
- * 2.004ms = full "forward"
- * 1.52ms = the "high end" of the deadband range
- * 1.50ms = center of the deadband range (off)
- * 1.48ms = the "low end" of the deadband range
- * 0.997ms = full "reverse"
- */
- SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_PWMVictorSPX, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_PWMVictorSPX, GetChannel() + 1);
SendableRegistry::GetInstance().SetName(this, "PWMVictorSPX", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp b/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
index b3297ea..7d7d916 100644
--- a/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
+++ b/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
@@ -7,7 +7,7 @@
#include "frc/PowerDistributionPanel.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/PDP.h>
#include <hal/Ports.h>
#include <wpi/SmallString.h>
@@ -29,12 +29,11 @@
int32_t status = 0;
m_handle = HAL_InitializePDP(module, &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumPDPModules(), module,
- HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumPDPModules(), module);
return;
}
- HAL_Report(HALUsageReporting::kResourceType_PDP, module);
+ HAL_Report(HALUsageReporting::kResourceType_PDP, module + 1);
SendableRegistry::GetInstance().AddLW(this, "PowerDistributionPanel", module);
}
diff --git a/wpilibc/src/main/native/cpp/Preferences.cpp b/wpilibc/src/main/native/cpp/Preferences.cpp
index 7a896d1..6014775 100644
--- a/wpilibc/src/main/native/cpp/Preferences.cpp
+++ b/wpilibc/src/main/native/cpp/Preferences.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-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. */
@@ -9,7 +9,7 @@
#include <algorithm>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <networktables/NetworkTableInstance.h>
#include <wpi/StringRef.h>
diff --git a/wpilibc/src/main/native/cpp/Relay.cpp b/wpilibc/src/main/native/cpp/Relay.cpp
index 3a3e772..cb95223 100644
--- a/wpilibc/src/main/native/cpp/Relay.cpp
+++ b/wpilibc/src/main/native/cpp/Relay.cpp
@@ -9,7 +9,8 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/Ports.h>
#include <hal/Relay.h>
#include <wpi/raw_ostream.h>
@@ -35,20 +36,18 @@
int32_t status = 0;
m_forwardHandle = HAL_InitializeRelayPort(portHandle, true, &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumRelayChannels(),
- channel, HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumRelayChannels(), channel);
m_forwardHandle = HAL_kInvalidHandle;
m_reverseHandle = HAL_kInvalidHandle;
return;
}
- HAL_Report(HALUsageReporting::kResourceType_Relay, m_channel);
+ HAL_Report(HALUsageReporting::kResourceType_Relay, m_channel + 1);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
int32_t status = 0;
m_reverseHandle = HAL_InitializeRelayPort(portHandle, false, &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumRelayChannels(),
- channel, HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumRelayChannels(), channel);
m_forwardHandle = HAL_kInvalidHandle;
m_reverseHandle = HAL_kInvalidHandle;
return;
@@ -61,7 +60,7 @@
if (m_forwardHandle != HAL_kInvalidHandle) {
HAL_SetRelay(m_forwardHandle, false, &status);
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_forwardHandle = HAL_kInvalidHandle;
m_reverseHandle = HAL_kInvalidHandle;
return;
@@ -70,7 +69,7 @@
if (m_reverseHandle != HAL_kInvalidHandle) {
HAL_SetRelay(m_reverseHandle, false, &status);
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_forwardHandle = HAL_kInvalidHandle;
m_reverseHandle = HAL_kInvalidHandle;
return;
@@ -137,7 +136,7 @@
break;
}
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
Relay::Value Relay::Get() const {
@@ -171,7 +170,7 @@
}
}
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int Relay::GetChannel() const { return m_channel; }
diff --git a/wpilibc/src/main/native/cpp/RobotController.cpp b/wpilibc/src/main/native/cpp/RobotController.cpp
index 347440d..b1d7608 100644
--- a/wpilibc/src/main/native/cpp/RobotController.cpp
+++ b/wpilibc/src/main/native/cpp/RobotController.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -7,7 +7,10 @@
#include "frc/RobotController.h"
-#include <hal/HAL.h>
+#include <hal/CAN.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <hal/Power.h>
#include "frc/ErrorBase.h"
@@ -16,21 +19,21 @@
int RobotController::GetFPGAVersion() {
int32_t status = 0;
int version = HAL_GetFPGAVersion(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return version;
}
int64_t RobotController::GetFPGARevision() {
int32_t status = 0;
int64_t revision = HAL_GetFPGARevision(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return revision;
}
uint64_t RobotController::GetFPGATime() {
int32_t status = 0;
uint64_t time = HAL_GetFPGATime(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return time;
}
@@ -46,112 +49,112 @@
bool RobotController::IsSysActive() {
int32_t status = 0;
bool retVal = HAL_GetSystemActive(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
bool RobotController::IsBrownedOut() {
int32_t status = 0;
bool retVal = HAL_GetBrownedOut(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
double RobotController::GetInputVoltage() {
int32_t status = 0;
double retVal = HAL_GetVinVoltage(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
double RobotController::GetInputCurrent() {
int32_t status = 0;
double retVal = HAL_GetVinCurrent(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
double RobotController::GetVoltage3V3() {
int32_t status = 0;
double retVal = HAL_GetUserVoltage3V3(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
double RobotController::GetCurrent3V3() {
int32_t status = 0;
double retVal = HAL_GetUserCurrent3V3(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
bool RobotController::GetEnabled3V3() {
int32_t status = 0;
bool retVal = HAL_GetUserActive3V3(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
int RobotController::GetFaultCount3V3() {
int32_t status = 0;
int retVal = HAL_GetUserCurrentFaults3V3(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
double RobotController::GetVoltage5V() {
int32_t status = 0;
double retVal = HAL_GetUserVoltage5V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
double RobotController::GetCurrent5V() {
int32_t status = 0;
double retVal = HAL_GetUserCurrent5V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
bool RobotController::GetEnabled5V() {
int32_t status = 0;
bool retVal = HAL_GetUserActive5V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
int RobotController::GetFaultCount5V() {
int32_t status = 0;
int retVal = HAL_GetUserCurrentFaults5V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
double RobotController::GetVoltage6V() {
int32_t status = 0;
double retVal = HAL_GetUserVoltage6V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
double RobotController::GetCurrent6V() {
int32_t status = 0;
double retVal = HAL_GetUserCurrent6V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
bool RobotController::GetEnabled6V() {
int32_t status = 0;
bool retVal = HAL_GetUserActive6V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
int RobotController::GetFaultCount6V() {
int32_t status = 0;
int retVal = HAL_GetUserCurrentFaults6V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
@@ -165,7 +168,7 @@
HAL_CAN_GetCANStatus(&percentBusUtilization, &busOffCount, &txFullCount,
&receiveErrorCount, &transmitErrorCount, &status);
if (status != 0) {
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return {};
}
return {percentBusUtilization, static_cast<int>(busOffCount),
diff --git a/wpilibc/src/main/native/cpp/RobotDrive.cpp b/wpilibc/src/main/native/cpp/RobotDrive.cpp
index fd43c96..5235fa2 100644
--- a/wpilibc/src/main/native/cpp/RobotDrive.cpp
+++ b/wpilibc/src/main/native/cpp/RobotDrive.cpp
@@ -10,7 +10,7 @@
#include <algorithm>
#include <cmath>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/GenericHID.h"
#include "frc/Joystick.h"
@@ -128,13 +128,13 @@
if (curve < 0) {
double value = std::log(-curve);
double ratio = (value - m_sensitivity) / (value + m_sensitivity);
- if (ratio == 0) ratio = .0000000001;
+ if (ratio == 0) ratio = 0.0000000001;
leftOutput = outputMagnitude / ratio;
rightOutput = outputMagnitude;
} else if (curve > 0) {
double value = std::log(curve);
double ratio = (value - m_sensitivity) / (value + m_sensitivity);
- if (ratio == 0) ratio = .0000000001;
+ if (ratio == 0) ratio = 0.0000000001;
leftOutput = outputMagnitude;
rightOutput = outputMagnitude / ratio;
} else {
diff --git a/wpilibc/src/main/native/cpp/SD540.cpp b/wpilibc/src/main/native/cpp/SD540.cpp
index 93733b2..9611f66 100644
--- a/wpilibc/src/main/native/cpp/SD540.cpp
+++ b/wpilibc/src/main/native/cpp/SD540.cpp
@@ -7,31 +7,19 @@
#include "frc/SD540.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
SD540::SD540(int channel) : PWMSpeedController(channel) {
- /* 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.
- *
- * 2.05ms = full "forward"
- * 1.55ms = the "high end" of the deadband range
- * 1.50ms = center of the deadband range (off)
- * 1.44ms = the "low end" of the deadband range
- * 0.94ms = full "reverse"
- */
- SetBounds(2.05, 1.55, 1.50, 1.44, .94);
+ SetBounds(2.05, 1.55, 1.50, 1.44, 0.94);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_MindsensorsSD540, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_MindsensorsSD540,
+ GetChannel() + 1);
SendableRegistry::GetInstance().SetName(this, "SD540", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/SPI.cpp b/wpilibc/src/main/native/cpp/SPI.cpp
index 54cf82b..074cc79 100644
--- a/wpilibc/src/main/native/cpp/SPI.cpp
+++ b/wpilibc/src/main/native/cpp/SPI.cpp
@@ -10,7 +10,7 @@
#include <cstring>
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/SPI.h>
#include <wpi/SmallVector.h>
#include <wpi/mutex.h>
@@ -155,9 +155,10 @@
SPI::SPI(Port port) : m_port(static_cast<HAL_SPIPort>(port)) {
int32_t status = 0;
HAL_InitializeSPI(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
- HAL_Report(HALUsageReporting::kResourceType_SPI, port);
+ HAL_Report(HALUsageReporting::kResourceType_SPI,
+ static_cast<uint8_t>(port) + 1);
}
SPI::~SPI() { HAL_CloseSPI(m_port); }
@@ -207,13 +208,13 @@
void SPI::SetChipSelectActiveHigh() {
int32_t status = 0;
HAL_SetSPIChipSelectActiveHigh(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SPI::SetChipSelectActiveLow() {
int32_t status = 0;
HAL_SetSPIChipSelectActiveLow(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int SPI::Write(uint8_t* data, int size) {
@@ -243,26 +244,26 @@
void SPI::InitAuto(int bufferSize) {
int32_t status = 0;
HAL_InitSPIAuto(m_port, bufferSize, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SPI::FreeAuto() {
int32_t status = 0;
HAL_FreeSPIAuto(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SPI::SetAutoTransmitData(wpi::ArrayRef<uint8_t> dataToSend, int zeroSize) {
int32_t status = 0;
HAL_SetSPIAutoTransmitData(m_port, dataToSend.data(), dataToSend.size(),
zeroSize, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SPI::StartAutoRate(units::second_t period) {
int32_t status = 0;
HAL_StartSPIAutoRate(m_port, period.to<double>(), &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SPI::StartAutoRate(double period) {
@@ -275,19 +276,19 @@
m_port, source.GetPortHandleForRouting(),
(HAL_AnalogTriggerType)source.GetAnalogTriggerTypeForRouting(), rising,
falling, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SPI::StopAuto() {
int32_t status = 0;
HAL_StopSPIAuto(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SPI::ForceAutoRead() {
int32_t status = 0;
HAL_ForceSPIAutoRead(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int SPI::ReadAutoReceivedData(uint32_t* buffer, int numToRead,
@@ -295,7 +296,7 @@
int32_t status = 0;
int32_t val = HAL_ReadSPIAutoReceivedData(m_port, buffer, numToRead,
timeout.to<double>(), &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return val;
}
@@ -306,7 +307,7 @@
int SPI::GetAutoDroppedCount() {
int32_t status = 0;
int32_t val = HAL_GetSPIAutoDroppedCount(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return val;
}
diff --git a/wpilibc/src/main/native/cpp/SensorUtil.cpp b/wpilibc/src/main/native/cpp/SensorUtil.cpp
index 2012da4..8c02f1b 100644
--- a/wpilibc/src/main/native/cpp/SensorUtil.cpp
+++ b/wpilibc/src/main/native/cpp/SensorUtil.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -10,7 +10,7 @@
#include <hal/AnalogInput.h>
#include <hal/AnalogOutput.h>
#include <hal/DIO.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/PDP.h>
#include <hal/PWM.h>
#include <hal/Ports.h>
diff --git a/wpilibc/src/main/native/cpp/SerialPort.cpp b/wpilibc/src/main/native/cpp/SerialPort.cpp
index 46e02c8..e092fc2 100644
--- a/wpilibc/src/main/native/cpp/SerialPort.cpp
+++ b/wpilibc/src/main/native/cpp/SerialPort.cpp
@@ -9,7 +9,7 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/SerialPort.h>
using namespace frc;
@@ -21,17 +21,17 @@
m_portHandle =
HAL_InitializeSerialPort(static_cast<HAL_SerialPort>(port), &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
// Don't continue if initialization failed
if (status < 0) return;
HAL_SetSerialBaudRate(m_portHandle, baudRate, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_SetSerialDataBits(m_portHandle, dataBits, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_SetSerialParity(m_portHandle, parity, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_SetSerialStopBits(m_portHandle, stopBits, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
// Set the default timeout to 5 seconds.
SetTimeout(5.0);
@@ -41,7 +41,8 @@
DisableTermination();
- HAL_Report(HALUsageReporting::kResourceType_SerialPort, 0);
+ HAL_Report(HALUsageReporting::kResourceType_SerialPort,
+ static_cast<uint8_t>(port) + 1);
}
SerialPort::SerialPort(int baudRate, const wpi::Twine& portName, Port port,
@@ -54,17 +55,17 @@
m_portHandle = HAL_InitializeSerialPortDirect(
static_cast<HAL_SerialPort>(port), portNameC, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
// Don't continue if initialization failed
if (status < 0) return;
HAL_SetSerialBaudRate(m_portHandle, baudRate, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_SetSerialDataBits(m_portHandle, dataBits, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_SetSerialParity(m_portHandle, parity, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_SetSerialStopBits(m_portHandle, stopBits, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
// Set the default timeout to 5 seconds.
SetTimeout(5.0);
@@ -74,44 +75,45 @@
DisableTermination();
- HAL_Report(HALUsageReporting::kResourceType_SerialPort, 0);
+ HAL_Report(HALUsageReporting::kResourceType_SerialPort,
+ static_cast<uint8_t>(port) + 1);
}
SerialPort::~SerialPort() {
int32_t status = 0;
HAL_CloseSerial(m_portHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SerialPort::SetFlowControl(SerialPort::FlowControl flowControl) {
int32_t status = 0;
HAL_SetSerialFlowControl(m_portHandle, flowControl, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SerialPort::EnableTermination(char terminator) {
int32_t status = 0;
HAL_EnableSerialTermination(m_portHandle, terminator, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SerialPort::DisableTermination() {
int32_t status = 0;
HAL_DisableSerialTermination(m_portHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int SerialPort::GetBytesReceived() {
int32_t status = 0;
int retVal = HAL_GetSerialBytesReceived(m_portHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return retVal;
}
int SerialPort::Read(char* buffer, int count) {
int32_t status = 0;
int retVal = HAL_ReadSerial(m_portHandle, buffer, count, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return retVal;
}
@@ -123,42 +125,42 @@
int32_t status = 0;
int retVal =
HAL_WriteSerial(m_portHandle, buffer.data(), buffer.size(), &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return retVal;
}
void SerialPort::SetTimeout(double timeout) {
int32_t status = 0;
HAL_SetSerialTimeout(m_portHandle, timeout, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SerialPort::SetReadBufferSize(int size) {
int32_t status = 0;
HAL_SetSerialReadBufferSize(m_portHandle, size, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SerialPort::SetWriteBufferSize(int size) {
int32_t status = 0;
HAL_SetSerialWriteBufferSize(m_portHandle, size, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SerialPort::SetWriteBufferMode(SerialPort::WriteBufferMode mode) {
int32_t status = 0;
HAL_SetSerialWriteMode(m_portHandle, mode, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SerialPort::Flush() {
int32_t status = 0;
HAL_FlushSerial(m_portHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SerialPort::Reset() {
int32_t status = 0;
HAL_ClearSerial(m_portHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
diff --git a/wpilibc/src/main/native/cpp/Servo.cpp b/wpilibc/src/main/native/cpp/Servo.cpp
index 09e482b..4b6856a 100644
--- a/wpilibc/src/main/native/cpp/Servo.cpp
+++ b/wpilibc/src/main/native/cpp/Servo.cpp
@@ -7,7 +7,7 @@
#include "frc/Servo.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableBuilder.h"
#include "frc/smartdashboard/SendableRegistry.h"
@@ -27,7 +27,7 @@
// Assign defaults for period multiplier for the servo PWM control signal
SetPeriodMultiplier(kPeriodMultiplier_4X);
- HAL_Report(HALUsageReporting::kResourceType_Servo, channel);
+ HAL_Report(HALUsageReporting::kResourceType_Servo, channel + 1);
SendableRegistry::GetInstance().SetName(this, "Servo", channel);
}
diff --git a/wpilibc/src/main/native/cpp/Solenoid.cpp b/wpilibc/src/main/native/cpp/Solenoid.cpp
index c860c44..e0bde3c 100644
--- a/wpilibc/src/main/native/cpp/Solenoid.cpp
+++ b/wpilibc/src/main/native/cpp/Solenoid.cpp
@@ -9,7 +9,8 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/Ports.h>
#include <hal/Solenoid.h>
@@ -40,14 +41,13 @@
m_solenoidHandle = HAL_InitializeSolenoidPort(
HAL_GetPortWithModule(moduleNumber, channel), &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumSolenoidChannels(),
- channel, HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumSolenoidChannels(), channel);
m_solenoidHandle = HAL_kInvalidHandle;
return;
}
- HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_channel,
- m_moduleNumber);
+ HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_channel + 1,
+ m_moduleNumber + 1);
SendableRegistry::GetInstance().AddLW(this, "Solenoid", m_moduleNumber,
m_channel);
}
@@ -58,14 +58,14 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetSolenoid(m_solenoidHandle, on, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
bool Solenoid::Get() const {
if (StatusIsFatal()) return false;
int32_t status = 0;
bool value = HAL_GetSolenoid(m_solenoidHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -79,14 +79,14 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetOneShotDuration(m_solenoidHandle, durationMS, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Solenoid::StartPulse() {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_FireOneShot(m_solenoidHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Solenoid::InitSendable(SendableBuilder& builder) {
diff --git a/wpilibc/src/main/native/cpp/SolenoidBase.cpp b/wpilibc/src/main/native/cpp/SolenoidBase.cpp
index c0b79a5..f5f8aad 100644
--- a/wpilibc/src/main/native/cpp/SolenoidBase.cpp
+++ b/wpilibc/src/main/native/cpp/SolenoidBase.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -7,7 +7,7 @@
#include "frc/SolenoidBase.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/Solenoid.h>
using namespace frc;
@@ -16,7 +16,7 @@
int value = 0;
int32_t status = 0;
value = HAL_GetAllSolenoids(module, &status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return value;
}
diff --git a/wpilibc/src/main/native/cpp/Spark.cpp b/wpilibc/src/main/native/cpp/Spark.cpp
index 18f8ee6..3717df4 100644
--- a/wpilibc/src/main/native/cpp/Spark.cpp
+++ b/wpilibc/src/main/native/cpp/Spark.cpp
@@ -7,31 +7,18 @@
#include "frc/Spark.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
Spark::Spark(int channel) : PWMSpeedController(channel) {
- /* 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.
- *
- * 2.003ms = full "forward"
- * 1.55ms = the "high end" of the deadband range
- * 1.50ms = center of the deadband range (off)
- * 1.46ms = the "low end" of the deadband range
- * 0.999ms = full "reverse"
- */
- SetBounds(2.003, 1.55, 1.50, 1.46, .999);
+ SetBounds(2.003, 1.55, 1.50, 1.46, 0.999);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_RevSPARK, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_RevSPARK, GetChannel() + 1);
SendableRegistry::GetInstance().SetName(this, "Spark", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp b/wpilibc/src/main/native/cpp/SpeedController.cpp
similarity index 65%
copy from wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
copy to wpilibc/src/main/native/cpp/SpeedController.cpp
index 8bd62ea..cb3cc5b 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
+++ b/wpilibc/src/main/native/cpp/SpeedController.cpp
@@ -5,12 +5,12 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include "frc2/command/PrintCommand.h"
+#include "frc/SpeedController.h"
-using namespace frc2;
+#include <frc/RobotController.h>
-PrintCommand::PrintCommand(const wpi::Twine& message)
- : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
+using namespace frc;
+
+void SpeedController::SetVoltage(units::volt_t output) {
+ Set(output / units::volt_t(RobotController::GetInputVoltage()));
}
-
-bool PrintCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibc/src/main/native/cpp/Talon.cpp b/wpilibc/src/main/native/cpp/Talon.cpp
index 8dc1928..4807d71 100644
--- a/wpilibc/src/main/native/cpp/Talon.cpp
+++ b/wpilibc/src/main/native/cpp/Talon.cpp
@@ -7,31 +7,18 @@
#include "frc/Talon.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
Talon::Talon(int channel) : PWMSpeedController(channel) {
- /* 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.
- *
- * 2.037ms = full "forward"
- * 1.539ms = the "high end" of the deadband range
- * 1.513ms = center of the deadband range (off)
- * 1.487ms = the "low end" of the deadband range
- * 0.989ms = full "reverse"
- */
- SetBounds(2.037, 1.539, 1.513, 1.487, .989);
+ SetBounds(2.037, 1.539, 1.513, 1.487, 0.989);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_Talon, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_Talon, GetChannel() + 1);
SendableRegistry::GetInstance().SetName(this, "Talon", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/Threads.cpp b/wpilibc/src/main/native/cpp/Threads.cpp
index 798e86a..7b1e647 100644
--- a/wpilibc/src/main/native/cpp/Threads.cpp
+++ b/wpilibc/src/main/native/cpp/Threads.cpp
@@ -7,7 +7,7 @@
#include "frc/Threads.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/Threads.h>
#include "frc/ErrorBase.h"
@@ -19,7 +19,7 @@
HAL_Bool rt = false;
auto native = thread.native_handle();
auto ret = HAL_GetThreadPriority(&native, &rt, &status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
*isRealTime = rt;
return ret;
}
@@ -28,7 +28,7 @@
int32_t status = 0;
HAL_Bool rt = false;
auto ret = HAL_GetCurrentThreadPriority(&rt, &status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
*isRealTime = rt;
return ret;
}
@@ -37,14 +37,14 @@
int32_t status = 0;
auto native = thread.native_handle();
auto ret = HAL_SetThreadPriority(&native, realTime, priority, &status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return ret;
}
bool SetCurrentThreadPriority(bool realTime, int priority) {
int32_t status = 0;
auto ret = HAL_SetCurrentThreadPriority(realTime, priority, &status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return ret;
}
diff --git a/wpilibc/src/main/native/cpp/TimedRobot.cpp b/wpilibc/src/main/native/cpp/TimedRobot.cpp
index ffff2dd..a9358dd 100644
--- a/wpilibc/src/main/native/cpp/TimedRobot.cpp
+++ b/wpilibc/src/main/native/cpp/TimedRobot.cpp
@@ -11,7 +11,9 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/Notifier.h>
#include "frc/Timer.h"
#include "frc/Utility.h"
@@ -43,6 +45,11 @@
}
}
+void TimedRobot::EndCompetition() {
+ int32_t status = 0;
+ HAL_StopNotifier(m_notifier, &status);
+}
+
units::second_t TimedRobot::GetPeriod() const {
return units::second_t(m_period);
}
@@ -52,7 +59,8 @@
TimedRobot::TimedRobot(units::second_t period) : IterativeRobotBase(period) {
int32_t status = 0;
m_notifier = HAL_InitializeNotifier(&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
+ HAL_SetNotifierName(m_notifier, "TimedRobot", &status);
HAL_Report(HALUsageReporting::kResourceType_Framework,
HALUsageReporting::kFramework_Timed);
@@ -62,7 +70,7 @@
int32_t status = 0;
HAL_StopNotifier(m_notifier, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_CleanNotifier(m_notifier, &status);
}
@@ -71,5 +79,5 @@
int32_t status = 0;
HAL_UpdateNotifierAlarm(
m_notifier, static_cast<uint64_t>(m_expirationTime * 1e6), &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
diff --git a/wpilibc/src/main/native/cpp/Timer.cpp b/wpilibc/src/main/native/cpp/Timer.cpp
index cdfa9ab..c91bc13 100644
--- a/wpilibc/src/main/native/cpp/Timer.cpp
+++ b/wpilibc/src/main/native/cpp/Timer.cpp
@@ -10,7 +10,7 @@
#include <chrono>
#include <thread>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/DriverStation.h"
#include "frc/RobotController.h"
diff --git a/wpilibc/src/main/native/cpp/Ultrasonic.cpp b/wpilibc/src/main/native/cpp/Ultrasonic.cpp
index 35320de..fcd016e 100644
--- a/wpilibc/src/main/native/cpp/Ultrasonic.cpp
+++ b/wpilibc/src/main/native/cpp/Ultrasonic.cpp
@@ -7,7 +7,7 @@
#include "frc/Ultrasonic.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/Counter.h"
#include "frc/DigitalInput.h"
diff --git a/wpilibc/src/main/native/cpp/Utility.cpp b/wpilibc/src/main/native/cpp/Utility.cpp
index 499f9b4..11f6234 100644
--- a/wpilibc/src/main/native/cpp/Utility.cpp
+++ b/wpilibc/src/main/native/cpp/Utility.cpp
@@ -17,7 +17,7 @@
#include <cstring>
#include <hal/DriverStation.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <wpi/Path.h>
#include <wpi/SmallString.h>
#include <wpi/StackTrace.h>
diff --git a/wpilibc/src/main/native/cpp/Victor.cpp b/wpilibc/src/main/native/cpp/Victor.cpp
index 49dcd57..bce1913 100644
--- a/wpilibc/src/main/native/cpp/Victor.cpp
+++ b/wpilibc/src/main/native/cpp/Victor.cpp
@@ -7,32 +7,18 @@
#include "frc/Victor.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
Victor::Victor(int channel) : PWMSpeedController(channel) {
- /* 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 as well 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 IFI.
- *
- * 2.027ms = full "forward"
- * 1.525ms = the "high end" of the deadband range
- * 1.507ms = center of the deadband range (off)
- * 1.49ms = the "low end" of the deadband range
- * 1.026ms = full "reverse"
- */
SetBounds(2.027, 1.525, 1.507, 1.49, 1.026);
SetPeriodMultiplier(kPeriodMultiplier_2X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_Victor, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_Victor, GetChannel() + 1);
SendableRegistry::GetInstance().SetName(this, "Victor", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/VictorSP.cpp b/wpilibc/src/main/native/cpp/VictorSP.cpp
index 82e966f..221777d 100644
--- a/wpilibc/src/main/native/cpp/VictorSP.cpp
+++ b/wpilibc/src/main/native/cpp/VictorSP.cpp
@@ -7,31 +7,18 @@
#include "frc/VictorSP.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
VictorSP::VictorSP(int channel) : PWMSpeedController(channel) {
- /* 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 Vex.
- *
- * 2.004ms = full "forward"
- * 1.52ms = the "high end" of the deadband range
- * 1.50ms = center of the deadband range (off)
- * 1.48ms = the "low end" of the deadband range
- * 0.997ms = full "reverse"
- */
- SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_VictorSP, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_VictorSP, GetChannel() + 1);
SendableRegistry::GetInstance().SetName(this, "VictorSP", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/XboxController.cpp b/wpilibc/src/main/native/cpp/XboxController.cpp
index fa54980..b294257 100644
--- a/wpilibc/src/main/native/cpp/XboxController.cpp
+++ b/wpilibc/src/main/native/cpp/XboxController.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -7,12 +7,12 @@
#include "frc/XboxController.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
using namespace frc;
XboxController::XboxController(int port) : GenericHID(port) {
- HAL_Report(HALUsageReporting::kResourceType_XboxController, port);
+ HAL_Report(HALUsageReporting::kResourceType_XboxController, port + 1);
}
double XboxController::GetX(JoystickHand hand) const {
diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
index 2202d93..264859f 100644
--- a/wpilibc/src/main/native/cpp/controller/PIDController.cpp
+++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
@@ -10,7 +10,7 @@
#include <algorithm>
#include <cmath>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableBuilder.h"
#include "frc/smartdashboard/SendableRegistry.h"
diff --git a/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp b/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
deleted file mode 100644
index 4ef3bf9..0000000
--- a/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
+++ /dev/null
@@ -1,133 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/controller/ProfiledPIDController.h"
-
-#include <algorithm>
-#include <cmath>
-
-#include "frc/smartdashboard/SendableBuilder.h"
-
-using namespace frc;
-
-ProfiledPIDController::ProfiledPIDController(
- double Kp, double Ki, double Kd,
- frc::TrapezoidProfile::Constraints constraints, units::second_t period)
- : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}
-
-void ProfiledPIDController::SetP(double Kp) { m_controller.SetP(Kp); }
-
-void ProfiledPIDController::SetI(double Ki) { m_controller.SetI(Ki); }
-
-void ProfiledPIDController::SetD(double Kd) { m_controller.SetD(Kd); }
-
-double ProfiledPIDController::GetP() const { return m_controller.GetP(); }
-
-double ProfiledPIDController::GetI() const { return m_controller.GetI(); }
-
-double ProfiledPIDController::GetD() const { return m_controller.GetD(); }
-
-units::second_t ProfiledPIDController::GetPeriod() const {
- return m_controller.GetPeriod();
-}
-
-void ProfiledPIDController::SetGoal(TrapezoidProfile::State goal) {
- m_goal = goal;
-}
-
-void ProfiledPIDController::SetGoal(units::meter_t goal) {
- m_goal = {goal, 0_mps};
-}
-
-TrapezoidProfile::State ProfiledPIDController::GetGoal() const {
- return m_goal;
-}
-
-bool ProfiledPIDController::AtGoal() const {
- return AtSetpoint() && m_goal == m_setpoint;
-}
-
-void ProfiledPIDController::SetConstraints(
- frc::TrapezoidProfile::Constraints constraints) {
- m_constraints = constraints;
-}
-
-TrapezoidProfile::State ProfiledPIDController::GetSetpoint() const {
- return m_setpoint;
-}
-
-bool ProfiledPIDController::AtSetpoint() const {
- return m_controller.AtSetpoint();
-}
-
-void ProfiledPIDController::EnableContinuousInput(double minimumInput,
- double maximumInput) {
- m_controller.EnableContinuousInput(minimumInput, maximumInput);
-}
-
-void ProfiledPIDController::DisableContinuousInput() {
- m_controller.DisableContinuousInput();
-}
-
-void ProfiledPIDController::SetIntegratorRange(double minimumIntegral,
- double maximumIntegral) {
- m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
-}
-
-void ProfiledPIDController::SetTolerance(double positionTolerance,
- double velocityTolerance) {
- m_controller.SetTolerance(positionTolerance, velocityTolerance);
-}
-
-double ProfiledPIDController::GetPositionError() const {
- return m_controller.GetPositionError();
-}
-
-double ProfiledPIDController::GetVelocityError() const {
- return m_controller.GetVelocityError();
-}
-
-double ProfiledPIDController::Calculate(units::meter_t measurement) {
- frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
- m_setpoint = profile.Calculate(GetPeriod());
- return m_controller.Calculate(measurement.to<double>(),
- m_setpoint.position.to<double>());
-}
-
-double ProfiledPIDController::Calculate(units::meter_t measurement,
- TrapezoidProfile::State goal) {
- SetGoal(goal);
- return Calculate(measurement);
-}
-
-double ProfiledPIDController::Calculate(units::meter_t measurement,
- units::meter_t goal) {
- SetGoal(goal);
- return Calculate(measurement);
-}
-
-double ProfiledPIDController::Calculate(
- units::meter_t measurement, units::meter_t goal,
- frc::TrapezoidProfile::Constraints constraints) {
- SetConstraints(constraints);
- return Calculate(measurement, goal);
-}
-
-void ProfiledPIDController::Reset() { m_controller.Reset(); }
-
-void ProfiledPIDController::InitSendable(frc::SendableBuilder& builder) {
- builder.SetSmartDashboardType("ProfiledPIDController");
- builder.AddDoubleProperty("p", [this] { return GetP(); },
- [this](double value) { SetP(value); });
- builder.AddDoubleProperty("i", [this] { return GetI(); },
- [this](double value) { SetI(value); });
- builder.AddDoubleProperty("d", [this] { return GetD(); },
- [this](double value) { SetD(value); });
- builder.AddDoubleProperty(
- "goal", [this] { return GetGoal().position.to<double>(); },
- [this](double value) { SetGoal(units::meter_t{value}); });
-}
diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
index ee23a5e..2d30a2b 100644
--- a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
@@ -10,7 +10,7 @@
#include <algorithm>
#include <cmath>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/SpeedController.h"
#include "frc/smartdashboard/SendableBuilder.h"
diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
index 4c15de8..e03951b 100644
--- a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
@@ -10,7 +10,7 @@
#include <algorithm>
#include <cmath>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <wpi/math>
#include "frc/SpeedController.h"
diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
index 473e033..0102d6a 100644
--- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
@@ -10,7 +10,7 @@
#include <algorithm>
#include <cmath>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <wpi/math>
#include "frc/SpeedController.h"
diff --git a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
index bae8868..fbce5a1 100644
--- a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
+++ b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
@@ -11,7 +11,7 @@
#include <cmath>
#include <cstddef>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/Base.h"
#include "frc/SpeedController.h"
diff --git a/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp b/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp
index 7dfc8f0..434cc27 100644
--- a/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp
+++ b/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp
@@ -10,7 +10,7 @@
#include <cassert>
#include <cmath>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
using namespace frc;
diff --git a/wpilibc/src/main/native/cpp/frc2/Timer.cpp b/wpilibc/src/main/native/cpp/frc2/Timer.cpp
index 126cfdd..76721bc 100644
--- a/wpilibc/src/main/native/cpp/frc2/Timer.cpp
+++ b/wpilibc/src/main/native/cpp/frc2/Timer.cpp
@@ -10,7 +10,7 @@
#include <chrono>
#include <thread>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/DriverStation.h"
#include "frc/RobotController.h"
@@ -129,7 +129,7 @@
units::second_t Timer::GetFPGATimestamp() {
// FPGA returns the timestamp in microseconds
- return units::second_t(frc::RobotController::GetFPGATime()) * 1.0e-6;
+ return units::second_t(frc::RobotController::GetFPGATime() * 1.0e-6);
}
units::second_t Timer::GetMatchTime() {
diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp
deleted file mode 100644
index 464a4a6..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp
+++ /dev/null
@@ -1,346 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/CommandScheduler.h"
-
-#include <frc/RobotState.h>
-#include <frc/WPIErrors.h>
-#include <frc/commands/Scheduler.h>
-#include <frc/smartdashboard/SendableBuilder.h>
-#include <frc/smartdashboard/SendableRegistry.h>
-#include <frc2/command/CommandGroupBase.h>
-#include <frc2/command/Subsystem.h>
-
-#include <hal/HAL.h>
-
-using namespace frc2;
-template <typename TMap, typename TKey>
-static bool ContainsKey(const TMap& map, TKey keyToCheck) {
- return map.find(keyToCheck) != map.end();
-}
-
-CommandScheduler::CommandScheduler() {
- frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler");
-}
-
-CommandScheduler& CommandScheduler::GetInstance() {
- static CommandScheduler scheduler;
- return scheduler;
-}
-
-void CommandScheduler::AddButton(wpi::unique_function<void()> button) {
- m_buttons.emplace_back(std::move(button));
-}
-
-void CommandScheduler::ClearButtons() { m_buttons.clear(); }
-
-void CommandScheduler::Schedule(bool interruptible, Command* command) {
- if (m_inRunLoop) {
- m_toSchedule.try_emplace(command, interruptible);
- return;
- }
-
- if (command->IsGrouped()) {
- wpi_setWPIErrorWithContext(CommandIllegalUse,
- "A command that is part of a command group "
- "cannot be independently scheduled");
- return;
- }
- if (m_disabled ||
- (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) ||
- ContainsKey(m_scheduledCommands, command)) {
- return;
- }
-
- const auto& requirements = command->GetRequirements();
-
- wpi::SmallVector<Command*, 8> intersection;
-
- bool isDisjoint = true;
- bool allInterruptible = true;
- for (auto&& i1 : m_requirements) {
- if (requirements.find(i1.first) != requirements.end()) {
- isDisjoint = false;
- allInterruptible &= m_scheduledCommands[i1.second].IsInterruptible();
- intersection.emplace_back(i1.second);
- }
- }
-
- if (isDisjoint || allInterruptible) {
- if (allInterruptible) {
- for (auto&& cmdToCancel : intersection) {
- Cancel(cmdToCancel);
- }
- }
- command->Initialize();
- m_scheduledCommands[command] = CommandState{interruptible};
- for (auto&& action : m_initActions) {
- action(*command);
- }
- for (auto&& requirement : requirements) {
- m_requirements[requirement] = command;
- }
- }
-}
-
-void CommandScheduler::Schedule(Command* command) { Schedule(true, command); }
-
-void CommandScheduler::Schedule(bool interruptible,
- wpi::ArrayRef<Command*> commands) {
- for (auto command : commands) {
- Schedule(interruptible, command);
- }
-}
-
-void CommandScheduler::Schedule(bool interruptible,
- std::initializer_list<Command*> commands) {
- for (auto command : commands) {
- Schedule(interruptible, command);
- }
-}
-
-void CommandScheduler::Schedule(wpi::ArrayRef<Command*> commands) {
- for (auto command : commands) {
- Schedule(true, command);
- }
-}
-
-void CommandScheduler::Schedule(std::initializer_list<Command*> commands) {
- for (auto command : commands) {
- Schedule(true, command);
- }
-}
-
-void CommandScheduler::Run() {
- if (m_disabled) {
- return;
- }
-
- // Run the periodic method of all registered subsystems.
- for (auto&& subsystem : m_subsystems) {
- subsystem.getFirst()->Periodic();
- }
-
- // Poll buttons for new commands to add.
- for (auto&& button : m_buttons) {
- button();
- }
-
- m_inRunLoop = true;
- // Run scheduled commands, remove finished commands.
- for (auto iterator = m_scheduledCommands.begin();
- iterator != m_scheduledCommands.end(); iterator++) {
- Command* command = iterator->getFirst();
-
- if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) {
- Cancel(command);
- continue;
- }
-
- command->Execute();
- for (auto&& action : m_executeActions) {
- action(*command);
- }
-
- if (command->IsFinished()) {
- command->End(false);
- for (auto&& action : m_finishActions) {
- action(*command);
- }
-
- for (auto&& requirement : command->GetRequirements()) {
- m_requirements.erase(requirement);
- }
-
- m_scheduledCommands.erase(iterator);
- }
- }
- m_inRunLoop = false;
-
- for (auto&& commandInterruptible : m_toSchedule) {
- Schedule(commandInterruptible.second, commandInterruptible.first);
- }
-
- for (auto&& command : m_toCancel) {
- Cancel(command);
- }
-
- m_toSchedule.clear();
- m_toCancel.clear();
-
- // Add default commands for un-required registered subsystems.
- for (auto&& subsystem : m_subsystems) {
- auto s = m_requirements.find(subsystem.getFirst());
- if (s == m_requirements.end()) {
- Schedule({subsystem.getSecond().get()});
- }
- }
-}
-
-void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) {
- m_subsystems[subsystem] = nullptr;
-}
-
-void CommandScheduler::UnregisterSubsystem(Subsystem* subsystem) {
- auto s = m_subsystems.find(subsystem);
- if (s != m_subsystems.end()) {
- m_subsystems.erase(s);
- }
-}
-
-void CommandScheduler::RegisterSubsystem(
- std::initializer_list<Subsystem*> subsystems) {
- for (auto* subsystem : subsystems) {
- RegisterSubsystem(subsystem);
- }
-}
-
-void CommandScheduler::UnregisterSubsystem(
- std::initializer_list<Subsystem*> subsystems) {
- for (auto* subsystem : subsystems) {
- UnregisterSubsystem(subsystem);
- }
-}
-
-Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
- auto&& find = m_subsystems.find(subsystem);
- if (find != m_subsystems.end()) {
- return find->second.get();
- } else {
- return nullptr;
- }
-}
-
-void CommandScheduler::Cancel(Command* command) {
- if (m_inRunLoop) {
- m_toCancel.emplace_back(command);
- return;
- }
-
- auto find = m_scheduledCommands.find(command);
- if (find == m_scheduledCommands.end()) return;
- command->End(true);
- for (auto&& action : m_interruptActions) {
- action(*command);
- }
- m_scheduledCommands.erase(find);
- for (auto&& requirement : m_requirements) {
- if (requirement.second == command) {
- m_requirements.erase(requirement.first);
- }
- }
-}
-
-void CommandScheduler::Cancel(wpi::ArrayRef<Command*> commands) {
- for (auto command : commands) {
- Cancel(command);
- }
-}
-
-void CommandScheduler::Cancel(std::initializer_list<Command*> commands) {
- for (auto command : commands) {
- Cancel(command);
- }
-}
-
-void CommandScheduler::CancelAll() {
- for (auto&& command : m_scheduledCommands) {
- Cancel(command.first);
- }
-}
-
-double CommandScheduler::TimeSinceScheduled(const Command* command) const {
- auto find = m_scheduledCommands.find(command);
- if (find != m_scheduledCommands.end()) {
- return find->second.TimeSinceInitialized();
- } else {
- return -1;
- }
-}
-bool CommandScheduler::IsScheduled(
- wpi::ArrayRef<const Command*> commands) const {
- for (auto command : commands) {
- if (!IsScheduled(command)) {
- return false;
- }
- }
- return true;
-}
-
-bool CommandScheduler::IsScheduled(
- std::initializer_list<const Command*> commands) const {
- for (auto command : commands) {
- if (!IsScheduled(command)) {
- return false;
- }
- }
- return true;
-}
-
-bool CommandScheduler::IsScheduled(const Command* command) const {
- return m_scheduledCommands.find(command) != m_scheduledCommands.end();
-}
-
-Command* CommandScheduler::Requiring(const Subsystem* subsystem) const {
- auto find = m_requirements.find(subsystem);
- if (find != m_requirements.end()) {
- return find->second;
- } else {
- return nullptr;
- }
-}
-
-void CommandScheduler::Disable() { m_disabled = true; }
-
-void CommandScheduler::Enable() { m_disabled = false; }
-
-void CommandScheduler::OnCommandInitialize(Action action) {
- m_initActions.emplace_back(std::move(action));
-}
-
-void CommandScheduler::OnCommandExecute(Action action) {
- m_executeActions.emplace_back(std::move(action));
-}
-
-void CommandScheduler::OnCommandInterrupt(Action action) {
- m_interruptActions.emplace_back(std::move(action));
-}
-
-void CommandScheduler::OnCommandFinish(Action action) {
- m_finishActions.emplace_back(std::move(action));
-}
-
-void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
- builder.SetSmartDashboardType("Scheduler");
- m_namesEntry = builder.GetEntry("Names");
- m_idsEntry = builder.GetEntry("Ids");
- m_cancelEntry = builder.GetEntry("Cancel");
-
- builder.SetUpdateTable([this] {
- double tmp[1];
- tmp[0] = 0;
- auto toCancel = m_cancelEntry.GetDoubleArray(tmp);
- for (auto cancel : toCancel) {
- uintptr_t ptrTmp = static_cast<uintptr_t>(cancel);
- Command* command = reinterpret_cast<Command*>(ptrTmp);
- if (m_scheduledCommands.find(command) != m_scheduledCommands.end()) {
- Cancel(command);
- }
- m_cancelEntry.SetDoubleArray(wpi::ArrayRef<double>{});
- }
-
- wpi::SmallVector<std::string, 8> names;
- wpi::SmallVector<double, 8> ids;
- for (auto&& command : m_scheduledCommands) {
- names.emplace_back(command.first->GetName());
- uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command.first);
- ids.emplace_back(static_cast<double>(ptrTmp));
- }
- m_namesEntry.SetStringArray(names);
- m_idsEntry.SetDoubleArray(ids);
- });
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp
deleted file mode 100644
index c0cc19b..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/ProfiledPIDCommand.h"
-
-using namespace frc2;
-using State = frc::TrapezoidProfile::State;
-
-ProfiledPIDCommand::ProfiledPIDCommand(
- frc::ProfiledPIDController controller,
- std::function<units::meter_t()> measurementSource,
- std::function<State()> goalSource,
- std::function<void(double, State)> useOutput,
- std::initializer_list<Subsystem*> requirements)
- : m_controller{controller},
- m_measurement{std::move(measurementSource)},
- m_goal{std::move(goalSource)},
- m_useOutput{std::move(useOutput)} {
- AddRequirements(requirements);
-}
-
-ProfiledPIDCommand::ProfiledPIDCommand(
- frc::ProfiledPIDController controller,
- std::function<units::meter_t()> measurementSource,
- std::function<units::meter_t()> goalSource,
- std::function<void(double, State)> useOutput,
- std::initializer_list<Subsystem*> requirements)
- : ProfiledPIDCommand(controller, measurementSource,
- [&goalSource]() {
- return State{goalSource(), 0_mps};
- },
- useOutput, requirements) {}
-
-ProfiledPIDCommand::ProfiledPIDCommand(
- frc::ProfiledPIDController controller,
- std::function<units::meter_t()> measurementSource, State goal,
- std::function<void(double, State)> useOutput,
- std::initializer_list<Subsystem*> requirements)
- : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
- useOutput, requirements) {}
-
-ProfiledPIDCommand::ProfiledPIDCommand(
- frc::ProfiledPIDController controller,
- std::function<units::meter_t()> measurementSource, units::meter_t goal,
- std::function<void(double, State)> useOutput,
- std::initializer_list<Subsystem*> requirements)
- : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
- useOutput, requirements) {}
-
-void ProfiledPIDCommand::Initialize() { m_controller.Reset(); }
-
-void ProfiledPIDCommand::Execute() {
- UseOutput(m_controller.Calculate(GetMeasurement(), m_goal()),
- m_controller.GetSetpoint());
-}
-
-void ProfiledPIDCommand::End(bool interrupted) {
- UseOutput(0, State{0_m, 0_mps});
-}
-
-units::meter_t ProfiledPIDCommand::GetMeasurement() { return m_measurement(); }
-
-void ProfiledPIDCommand::UseOutput(double output, State state) {
- m_useOutput(output, state);
-}
-
-frc::ProfiledPIDController& ProfiledPIDCommand::GetController() {
- return m_controller;
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp b/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp
deleted file mode 100644
index 469e153..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/ProfiledPIDSubsystem.h"
-
-using namespace frc2;
-using State = frc::TrapezoidProfile::State;
-
-ProfiledPIDSubsystem::ProfiledPIDSubsystem(
- frc::ProfiledPIDController controller)
- : m_controller{controller} {}
-
-void ProfiledPIDSubsystem::Periodic() {
- if (m_enabled) {
- UseOutput(m_controller.Calculate(GetMeasurement(), GetGoal()),
- m_controller.GetSetpoint());
- }
-}
-
-void ProfiledPIDSubsystem::Enable() {
- m_controller.Reset();
- m_enabled = true;
-}
-
-void ProfiledPIDSubsystem::Disable() {
- UseOutput(0, State{0_m, 0_mps});
- m_enabled = false;
-}
-
-frc::ProfiledPIDController& ProfiledPIDSubsystem::GetController() {
- return m_controller;
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp
deleted file mode 100644
index bb17edc..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/TrapezoidProfileCommand.h"
-
-#include <units/units.h>
-
-using namespace frc2;
-
-TrapezoidProfileCommand::TrapezoidProfileCommand(
- frc::TrapezoidProfile profile,
- std::function<void(frc::TrapezoidProfile::State)> output,
- std::initializer_list<Subsystem*> requirements)
- : m_profile(profile), m_output(output) {
- AddRequirements(requirements);
-}
-
-void TrapezoidProfileCommand::Initialize() {
- m_timer.Reset();
- m_timer.Start();
-}
-
-void TrapezoidProfileCommand::Execute() {
- m_output(m_profile.Calculate(m_timer.Get()));
-}
-
-void TrapezoidProfileCommand::End(bool interrupted) { m_timer.Stop(); }
-
-bool TrapezoidProfileCommand::IsFinished() {
- return m_timer.HasPeriodPassed(m_profile.TotalTime());
-}
diff --git a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
index 42d20f7..1754830 100644
--- a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
+++ b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
@@ -9,6 +9,8 @@
#include <cmath>
+#include <wpi/json.h>
+
using namespace frc;
Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
@@ -96,3 +98,13 @@
return {translationPart.X(), translationPart.Y(), units::radian_t(dtheta)};
}
+
+void frc::to_json(wpi::json& json, const Pose2d& pose) {
+ json = wpi::json{{"translation", pose.Translation()},
+ {"rotation", pose.Rotation()}};
+}
+
+void frc::from_json(const wpi::json& json, Pose2d& pose) {
+ pose = Pose2d{json.at("translation").get<Translation2d>(),
+ json.at("rotation").get<Rotation2d>()};
+}
diff --git a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
index 2723442..a3c4bea 100644
--- a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
+++ b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
@@ -9,6 +9,8 @@
#include <cmath>
+#include <wpi/json.h>
+
using namespace frc;
Rotation2d::Rotation2d(units::radian_t value)
@@ -68,3 +70,11 @@
return {Cos() * other.Cos() - Sin() * other.Sin(),
Cos() * other.Sin() + Sin() * other.Cos()};
}
+
+void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
+ json = wpi::json{{"radians", rotation.Radians().to<double>()}};
+}
+
+void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
+ rotation = Rotation2d{units::radian_t{json.at("radians").get<double>()}};
+}
diff --git a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
index ca287d1..c3db7e3 100644
--- a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
+++ b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
@@ -7,6 +7,8 @@
#include "frc/geometry/Translation2d.h"
+#include <wpi/json.h>
+
using namespace frc;
Translation2d::Translation2d(units::meter_t x, units::meter_t y)
@@ -73,3 +75,13 @@
*this *= (1.0 / scalar);
return *this;
}
+
+void frc::to_json(wpi::json& json, const Translation2d& translation) {
+ json = wpi::json{{"x", translation.X().to<double>()},
+ {"y", translation.Y().to<double>()}};
+}
+
+void frc::from_json(const wpi::json& json, Translation2d& translation) {
+ translation = Translation2d{units::meter_t{json.at("x").get<double>()},
+ units::meter_t{json.at("y").get<double>()}};
+}
diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp
deleted file mode 100644
index 8301481..0000000
--- a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-
-using namespace frc;
-
-DifferentialDriveKinematics::DifferentialDriveKinematics(
- units::meter_t trackWidth)
- : m_trackWidth(trackWidth) {}
-
-ChassisSpeeds DifferentialDriveKinematics::ToChassisSpeeds(
- const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
- return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
- (wheelSpeeds.right - wheelSpeeds.left) / m_trackWidth * 1_rad};
-}
-
-DifferentialDriveWheelSpeeds DifferentialDriveKinematics::ToWheelSpeeds(
- const ChassisSpeeds& chassisSpeeds) const {
- return {chassisSpeeds.vx - m_trackWidth / 2 * chassisSpeeds.omega / 1_rad,
- chassisSpeeds.vx + m_trackWidth / 2 * chassisSpeeds.omega / 1_rad};
-}
diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
index 418dd0f..8591da4 100644
--- a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
@@ -10,23 +10,26 @@
using namespace frc;
DifferentialDriveOdometry::DifferentialDriveOdometry(
- DifferentialDriveKinematics kinematics, const Pose2d& initialPose)
- : m_kinematics(kinematics), m_pose(initialPose) {
+ const Rotation2d& gyroAngle, const Pose2d& initialPose)
+ : m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
+ m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
-const Pose2d& DifferentialDriveOdometry::UpdateWithTime(
- units::second_t currentTime, const Rotation2d& angle,
- const DifferentialDriveWheelSpeeds& wheelSpeeds) {
- units::second_t deltaTime =
- (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
- m_previousTime = currentTime;
+const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
+ units::meter_t leftDistance,
+ units::meter_t rightDistance) {
+ auto deltaLeftDistance = leftDistance - m_prevLeftDistance;
+ auto deltaRightDistance = rightDistance - m_prevRightDistance;
- auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
- static_cast<void>(dtheta);
+ m_prevLeftDistance = leftDistance;
+ m_prevRightDistance = rightDistance;
+
+ auto averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
+ auto angle = gyroAngle + m_gyroOffset;
auto newPose = m_pose.Exp(
- {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
+ {averageDeltaDistance, 0_m, (angle - m_previousAngle).Radians()});
m_previousAngle = angle;
m_pose = {newPose.Translation(), angle};
diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
index 18e5635..615635a 100644
--- a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
@@ -10,18 +10,22 @@
using namespace frc;
MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
+ const Rotation2d& gyroAngle,
const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
+ m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
const Pose2d& MecanumDriveOdometry::UpdateWithTime(
- units::second_t currentTime, const Rotation2d& angle,
+ units::second_t currentTime, const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
units::second_t deltaTime =
(m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
m_previousTime = currentTime;
+ auto angle = gyroAngle + m_gyroOffset;
+
auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
static_cast<void>(dtheta);
diff --git a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
index 1f011f5..10996e4 100644
--- a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
+++ b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
@@ -12,7 +12,7 @@
#include <networktables/NetworkTableInstance.h>
#include <wpi/mutex.h>
-#include "frc/commands/Scheduler.h"
+#include "frc/smartdashboard/Sendable.h"
#include "frc/smartdashboard/SendableBuilderImpl.h"
#include "frc/smartdashboard/SendableRegistry.h"
@@ -99,19 +99,15 @@
void LiveWindow::SetEnabled(bool enabled) {
std::scoped_lock lock(m_impl->mutex);
if (m_impl->liveWindowEnabled == enabled) return;
- Scheduler* scheduler = Scheduler::GetInstance();
m_impl->startLiveWindow = enabled;
m_impl->liveWindowEnabled = enabled;
// Force table generation now to make sure everything is defined
UpdateValuesUnsafe();
if (enabled) {
- scheduler->SetEnabled(false);
- scheduler->RemoveAll();
} else {
m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
cbdata.builder.StopLiveWindowMode();
});
- scheduler->SetEnabled(true);
}
m_impl->enabledEntry.SetBoolean(enabled);
}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
index a50b77f..9717a8e 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
@@ -7,7 +7,7 @@
#include "frc/shuffleboard/ShuffleboardInstance.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableInstance.h>
#include <wpi/StringMap.h>
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
index 56a27dc..bce6196 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
@@ -42,7 +42,7 @@
}
};
- wpi::mutex mutex;
+ wpi::recursive_mutex mutex;
wpi::UidVector<std::unique_ptr<Component>, 32> components;
wpi::DenseMap<void*, UID> componentMap;
@@ -310,7 +310,9 @@
wpi::function_ref<void(CallbackData& data)> callback) const {
assert(dataHandle >= 0);
std::scoped_lock lock(m_impl->mutex);
- for (auto&& comp : m_impl->components) {
+ wpi::SmallVector<Impl::Component*, 128> components;
+ for (auto&& comp : m_impl->components) components.emplace_back(comp.get());
+ for (auto comp : components) {
if (comp->sendable && comp->liveWindow) {
if (static_cast<size_t>(dataHandle) >= comp->data.size())
comp->data.resize(dataHandle + 1);
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
index e8c9700..5e70a4c 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
@@ -7,7 +7,7 @@
#include "frc/smartdashboard/SmartDashboard.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableInstance.h>
#include <wpi/StringMap.h>
@@ -81,6 +81,10 @@
Singleton::GetInstance().table->Delete(key);
}
+nt::NetworkTableEntry SmartDashboard::GetEntry(wpi::StringRef key) {
+ return Singleton::GetInstance().table->GetEntry(key);
+}
+
void SmartDashboard::PutData(wpi::StringRef key, Sendable* data) {
if (data == nullptr) {
wpi_setGlobalWPIErrorWithContext(NullParameter, "value");
diff --git a/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
index 3578b1d..3991fec 100644
--- a/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
+++ b/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
@@ -27,10 +27,20 @@
// Populate Row 2 and Row 3 with the derivatives of the equations above.
// Then populate row 4 and 5 with the second derivatives.
for (int i = 0; i < 4; i++) {
+ // 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.template block<2, 1>(2, i) =
m_coefficients.template block<2, 1>(0, 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.template block<2, 1>(4, i) =
- m_coefficients.template block<2, 1>(2, i) * (3 - i);
+ m_coefficients.template block<2, 1>(2, i) * (2 - i);
}
}
diff --git a/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
index f714c6f..bb8ad3c 100644
--- a/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
+++ b/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
@@ -27,10 +27,19 @@
// Populate Row 2 and Row 3 with the derivatives of the equations above.
// Then populate row 4 and 5 with the second derivatives.
for (int i = 0; i < 6; i++) {
+ // 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.template block<2, 1>(2, i) =
m_coefficients.template block<2, 1>(0, i) * (5 - i);
-
+ }
+ for (int i = 0; i < 5; i++) {
+ // 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.template block<2, 1>(4, i) =
- m_coefficients.template block<2, 1>(2, i) * (5 - i);
+ m_coefficients.template block<2, 1>(2, i) * (4 - i);
}
}
diff --git a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
index 7c3fdc1..cbfc8c1 100644
--- a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
+++ b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
@@ -28,13 +28,25 @@
waypoints.emplace_back(
Translation2d{units::meter_t(xFinal[0]), units::meter_t(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
std::vector<double> a;
+ // Diagonal of tridiagonal matrix
std::vector<double> b(waypoints.size() - 2, 4.0);
+ // Below-diagonal of tridiagonal matrix, zero-padded
std::vector<double> c;
+ // rhs vectors
std::vector<double> dx, dy;
+ // solution vectors
std::vector<double> fx(waypoints.size() - 2, 0.0),
fy(waypoints.size() - 2, 0.0);
+ // populate above-diagonal and below-diagonal vectors
a.emplace_back(0);
for (size_t i = 0; i < waypoints.size() - 3; ++i) {
a.emplace_back(1);
@@ -42,6 +54,7 @@
}
c.emplace_back(0);
+ // populate rhs vectors
dx.emplace_back(
3 * (waypoints[2].X().to<double>() - waypoints[0].X().to<double>()) -
xInitial[1]);
@@ -63,6 +76,7 @@
waypoints[waypoints.size() - 3].Y().to<double>()) -
yFinal[1]);
+ // Compute solution to tridiagonal system
ThomasAlgorithm(a, b, c, dx, &fx);
ThomasAlgorithm(a, b, c, dy, &fy);
diff --git a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
index 9b4b4f5..7b2b2b6 100644
--- a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
+++ b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
@@ -7,8 +7,20 @@
#include "frc/trajectory/Trajectory.h"
+#include <wpi/json.h>
+
using namespace frc;
+bool Trajectory::State::operator==(const Trajectory::State& other) const {
+ return t == other.t && velocity == other.velocity &&
+ acceleration == other.acceleration && pose == other.pose &&
+ curvature == other.curvature;
+}
+
+bool Trajectory::State::operator!=(const Trajectory::State& other) const {
+ return !operator==(other);
+}
+
Trajectory::State Trajectory::State::Interpolate(State endValue,
double i) const {
// Find the new [t] value.
@@ -94,3 +106,21 @@
return prevSample.Interpolate(sample,
(t - prevSample.t) / (sample.t - prevSample.t));
}
+
+void frc::to_json(wpi::json& json, const Trajectory::State& state) {
+ json = wpi::json{{"time", state.t.to<double>()},
+ {"velocity", state.velocity.to<double>()},
+ {"acceleration", state.acceleration.to<double>()},
+ {"pose", state.pose},
+ {"curvature", state.curvature.to<double>()}};
+}
+
+void frc::from_json(const wpi::json& json, Trajectory::State& state) {
+ state.pose = json.at("pose").get<Pose2d>();
+ state.t = units::second_t{json.at("time").get<double>()};
+ state.velocity =
+ units::meters_per_second_t{json.at("velocity").get<double>()};
+ state.acceleration =
+ units::meters_per_second_squared_t{json.at("acceleration").get<double>()};
+ state.curvature = frc::curvature_t{json.at("curvature").get<double>()};
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
new file mode 100644
index 0000000..f3cf30d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrajectoryUtil.h"
+
+#include <system_error>
+
+#include <wpi/SmallString.h>
+#include <wpi/json.h>
+#include <wpi/raw_istream.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+void TrajectoryUtil::ToPathweaverJson(const Trajectory& trajectory,
+ const wpi::Twine& path) {
+ std::error_code error_code;
+
+ wpi::SmallString<128> buf;
+ wpi::raw_fd_ostream output{path.toStringRef(buf), error_code};
+ if (error_code) {
+ throw std::runtime_error(("Cannot open file: " + path).str());
+ }
+
+ wpi::json json = trajectory.States();
+ output << json;
+ output.flush();
+}
+
+Trajectory TrajectoryUtil::FromPathweaverJson(const wpi::Twine& path) {
+ std::error_code error_code;
+
+ wpi::SmallString<128> buf;
+ wpi::raw_fd_istream input{path.toStringRef(buf), error_code};
+ if (error_code) {
+ throw std::runtime_error(("Cannot open file: " + path).str());
+ }
+
+ wpi::json json;
+ input >> json;
+
+ return Trajectory{json.get<std::vector<Trajectory::State>>()};
+}
+
+std::string TrajectoryUtil::SerializeTrajectory(const Trajectory& trajectory) {
+ wpi::json json = trajectory.States();
+ return json.dump();
+}
+
+Trajectory TrajectoryUtil::DeserializeTrajectory(
+ const wpi::StringRef json_str) {
+ wpi::json json = wpi::json::parse(json_str);
+ return Trajectory{json.get<std::vector<Trajectory::State>>()};
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
new file mode 100644
index 0000000..3d6b61c
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
+
+#include <algorithm>
+#include <limits>
+
+#include <wpi/MathExtras.h>
+
+using namespace frc;
+
+DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint(
+ SimpleMotorFeedforward<units::meter> feedforward,
+ DifferentialDriveKinematics kinematics, units::volt_t maxVoltage)
+ : m_feedforward(feedforward),
+ m_kinematics(kinematics),
+ m_maxVoltage(maxVoltage) {}
+
+units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) {
+ return units::meters_per_second_t(std::numeric_limits<double>::max());
+}
+
+TrajectoryConstraint::MinMax
+DifferentialDriveVoltageConstraint::MinMaxAcceleration(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) {
+ auto wheelSpeeds =
+ m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature});
+
+ auto maxWheelSpeed = std::max(wheelSpeeds.left, wheelSpeeds.right);
+ auto minWheelSpeed = std::min(wheelSpeeds.left, wheelSpeeds.right);
+
+ // Calculate maximum/minimum possible accelerations from motor dynamics
+ // and max/min wheel speeds
+ auto maxWheelAcceleration =
+ m_feedforward.MaxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
+ auto 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
+ auto maxChassisAcceleration =
+ maxWheelAcceleration /
+ (1 + m_kinematics.trackWidth * units::math::abs(curvature) *
+ wpi::sgn(speed) / (2_rad));
+ auto minChassisAcceleration =
+ minWheelAcceleration /
+ (1 - m_kinematics.trackWidth * units::math::abs(curvature) *
+ wpi::sgn(speed) / (2_rad));
+
+ // Negate acceleration corresponding to wheel on inside of turn
+ // if center of turn is inside of wheelbase
+ if ((m_kinematics.trackWidth / 2) > 1_rad / units::math::abs(curvature)) {
+ if (speed > 0_mps) {
+ minChassisAcceleration = -minChassisAcceleration;
+ } else {
+ maxChassisAcceleration = -maxChassisAcceleration;
+ }
+ }
+
+ return {minChassisAcceleration, maxChassisAcceleration};
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
new file mode 100644
index 0000000..2fd8151
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
+
+using namespace frc;
+
+MecanumDriveKinematicsConstraint::MecanumDriveKinematicsConstraint(
+ MecanumDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
+ : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+
+units::meters_per_second_t MecanumDriveKinematicsConstraint::MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) {
+ auto xVelocity = velocity * pose.Rotation().Cos();
+ auto yVelocity = velocity * pose.Rotation().Sin();
+ auto wheelSpeeds =
+ m_kinematics.ToWheelSpeeds({xVelocity, yVelocity, velocity * curvature});
+ wheelSpeeds.Normalize(m_maxSpeed);
+
+ auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
+}
+
+TrajectoryConstraint::MinMax
+MecanumDriveKinematicsConstraint::MinMaxAcceleration(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) {
+ return {};
+}
diff --git a/wpilibc/src/main/native/cppcs/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp
index 65c072d..5fd824d 100644
--- a/wpilibc/src/main/native/cppcs/RobotBase.cpp
+++ b/wpilibc/src/main/native/cppcs/RobotBase.cpp
@@ -14,7 +14,8 @@
#include <cstdio>
#include <cameraserver/CameraServerShared.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <networktables/NetworkTableInstance.h>
#include "WPILibVersion.h"
@@ -117,11 +118,6 @@
std::thread::id RobotBase::GetThreadId() { return m_threadId; }
RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
- if (!HAL_Initialize(500, 0)) {
- wpi::errs() << "FATAL ERROR: HAL could not be initialized\n";
- wpi::errs().flush();
- std::terminate();
- }
m_threadId = std::this_thread::get_id();
SetupCameraServerShared();
diff --git a/wpilibc/src/main/native/include/frc/AddressableLED.h b/wpilibc/src/main/native/include/frc/AddressableLED.h
new file mode 100644
index 0000000..2ffe0ea
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/AddressableLED.h
@@ -0,0 +1,139 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <hal/AddressableLEDTypes.h>
+#include <hal/Types.h>
+#include <units/units.h>
+#include <wpi/ArrayRef.h>
+
+#include "frc/ErrorBase.h"
+
+namespace frc {
+
+/**
+ * A class for driving addressable LEDs, such as WS2812s and NeoPixels.
+ */
+class AddressableLED : public ErrorBase {
+ public:
+ class LEDData : public HAL_AddressableLEDData {
+ public:
+ LEDData() : LEDData(0, 0, 0) {}
+ LEDData(int _r, int _g, int _b) {
+ r = _r;
+ g = _g;
+ b = _b;
+ padding = 0;
+ }
+
+ /**
+ * A helper method to set all values of the LED.
+ *
+ * @param r the r value [0-255]
+ * @param g the g value [0-255]
+ * @param b the b value [0-255]
+ */
+ void SetRGB(int r, int g, int b) {
+ this->r = r;
+ this->g = g;
+ this->b = b;
+ }
+
+ /**
+ * A helper method to set all values of the LED.
+ *
+ * @param h the h value [0-180]
+ * @param s the s value [0-255]
+ * @param v the v value [0-255]
+ */
+ void SetHSV(int h, int s, int v);
+ };
+
+ /**
+ * Constructs a new driver for a specific port.
+ *
+ * @param port the output port to use (Must be a PWM port)
+ */
+ explicit AddressableLED(int port);
+
+ ~AddressableLED() override;
+
+ /**
+ * 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
+ */
+ void SetLength(int 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 ledData the buffer to write
+ */
+ void SetData(wpi::ArrayRef<LEDData> ledData);
+
+ /**
+ * 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 ledData the buffer to write
+ */
+ void SetData(std::initializer_list<LEDData> ledData);
+
+ /**
+ * Sets the bit timing.
+ *
+ * <p>By default, the driver is set up to drive WS2812s, so nothing needs to
+ * be set for those.
+ *
+ * @param lowTime0 low time for 0 bit
+ * @param highTime0 high time for 0 bit
+ * @param lowTime1 low time for 1 bit
+ * @param highTime1 high time for 1 bit
+ */
+ void SetBitTiming(units::nanosecond_t lowTime0, units::nanosecond_t highTime0,
+ units::nanosecond_t lowTime1,
+ units::nanosecond_t highTime1);
+
+ /**
+ * 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
+ */
+ void SetSyncTime(units::microsecond_t syncTime);
+
+ /**
+ * Starts the output.
+ *
+ * <p>The output writes continously.
+ */
+ void Start();
+
+ /**
+ * Stops the output.
+ */
+ void Stop();
+
+ private:
+ hal::Handle<HAL_DigitalHandle> m_pwmHandle;
+ hal::Handle<HAL_AddressableLEDHandle> m_handle;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h
new file mode 100644
index 0000000..872db87
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h
@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <hal/SimDevice.h>
+#include <hal/Types.h>
+#include <units/units.h>
+
+#include "frc/AnalogTrigger.h"
+#include "frc/Counter.h"
+#include "frc/ErrorBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+class AnalogInput;
+
+/**
+ * Class for supporting continuous analog encoders, such as the US Digital MA3.
+ */
+class AnalogEncoder : public ErrorBase,
+ public Sendable,
+ public SendableHelper<AnalogEncoder> {
+ public:
+ /**
+ * Construct a new AnalogEncoder attached to a specific AnalogInput.
+ *
+ * @param analogInput the analog input to attach to
+ */
+ explicit AnalogEncoder(AnalogInput& analogInput);
+
+ /**
+ * Construct a new AnalogEncoder attached to a specific AnalogInput.
+ *
+ * @param analogInput the analog input to attach to
+ */
+ explicit AnalogEncoder(AnalogInput* analogInput);
+
+ /**
+ * Construct a new AnalogEncoder attached to a specific AnalogInput.
+ *
+ * @param analogInput the analog input to attach to
+ */
+ explicit AnalogEncoder(std::shared_ptr<AnalogInput> analogInput);
+
+ ~AnalogEncoder() override = default;
+
+ AnalogEncoder(AnalogEncoder&&) = default;
+ AnalogEncoder& operator=(AnalogEncoder&&) = default;
+
+ /**
+ * Reset the Encoder distance to zero.
+ */
+ void Reset();
+
+ /**
+ * Get the encoder value since the last reset.
+ *
+ * This is reported in rotations since the last reset.
+ *
+ * @return the encoder value in rotations
+ */
+ units::turn_t Get() const;
+
+ /**
+ * Get the offset of position relative to the last reset.
+ *
+ * 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
+ */
+ double GetPositionOffset() const;
+
+ /**
+ * 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
+ */
+ void SetDistancePerRotation(double distancePerRotation);
+
+ /**
+ * Get the distance per rotation for this encoder.
+ *
+ * @return The scale factor that will be used to convert rotation to useful
+ * units.
+ */
+ double GetDistancePerRotation() const;
+
+ /**
+ * Get the distance the sensor has driven since the last reset as scaled by
+ * the value from SetDistancePerRotation.
+ *
+ * @return The distance driven since the last reset
+ */
+ double GetDistance() const;
+
+ void InitSendable(SendableBuilder& builder) override;
+
+ private:
+ void Init();
+
+ std::shared_ptr<AnalogInput> m_analogInput;
+ AnalogTrigger m_analogTrigger;
+ Counter m_counter;
+ double m_positionOffset = 0;
+ double m_distancePerRotation = 1.0;
+ mutable units::turn_t m_lastPosition{0.0};
+
+ hal::SimDevice m_simDevice;
+ hal::SimDouble m_simPosition;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/AnalogInput.h b/wpilibc/src/main/native/include/frc/AnalogInput.h
index 6567542..3d14c4c 100644
--- a/wpilibc/src/main/native/include/frc/AnalogInput.h
+++ b/wpilibc/src/main/native/include/frc/AnalogInput.h
@@ -19,6 +19,8 @@
namespace frc {
class SendableBuilder;
+class DMA;
+class DMASample;
/**
* Analog input class.
@@ -38,6 +40,8 @@
public SendableHelper<AnalogInput> {
friend class AnalogTrigger;
friend class AnalogGyro;
+ friend class DMA;
+ friend class DMASample;
public:
static constexpr int kAccumulatorModuleNumber = 1;
diff --git a/wpilibc/src/main/native/include/frc/AnalogTrigger.h b/wpilibc/src/main/native/include/frc/AnalogTrigger.h
index 6a57f8a..7554bd9 100644
--- a/wpilibc/src/main/native/include/frc/AnalogTrigger.h
+++ b/wpilibc/src/main/native/include/frc/AnalogTrigger.h
@@ -19,6 +19,7 @@
namespace frc {
class AnalogInput;
+class DutyCycle;
class SendableBuilder;
class AnalogTrigger : public ErrorBase,
@@ -45,6 +46,13 @@
*/
explicit AnalogTrigger(AnalogInput* channel);
+ /**
+ * Construct an analog trigger given a duty cycle input.
+ *
+ * @param channel The pointer to the existing DutyCycle object
+ */
+ explicit AnalogTrigger(DutyCycle* dutyCycle);
+
~AnalogTrigger() override;
AnalogTrigger(AnalogTrigger&& rhs);
@@ -61,6 +69,16 @@
void SetLimitsVoltage(double lower, double upper);
/**
+ * Set the upper and lower duty cycle limits of the analog trigger.
+ *
+ * The limits are given as floating point values between 0 and 1.
+ *
+ * @param lower The lower limit of the trigger in percentage.
+ * @param upper The upper limit of the trigger in percentage.
+ */
+ void SetLimitsDutyCycle(double lower, double upper);
+
+ /**
* Set the upper and lower limits of the analog trigger.
*
* The limits are given in ADC codes. If oversampling is used, the units must
@@ -83,6 +101,17 @@
void SetAveraged(bool useAveragedValue);
/**
+ * Configure the analog trigger to use the duty cycle vs. raw values.
+ *
+ * If the value is true, then the duty cycle value is selected for the analog
+ * trigger, otherwise the immediate value is used.
+ *
+ * @param useDutyCycle If true, use the duty cycle value, otherwise use the
+ * instantaneous reading
+ */
+ void SetDutyCycle(bool useDutyCycle);
+
+ /**
* Configure the analog trigger to use a filtered value.
*
* The analog trigger will operate with a 3 point average rejection filter.
@@ -139,9 +168,9 @@
void InitSendable(SendableBuilder& builder) override;
private:
- int m_index;
hal::Handle<HAL_AnalogTriggerHandle> m_trigger;
AnalogInput* m_analogInput = nullptr;
+ DutyCycle* m_dutyCycle = nullptr;
bool m_ownsAnalog = false;
};
diff --git a/wpilibc/src/main/native/include/frc/Counter.h b/wpilibc/src/main/native/include/frc/Counter.h
index 50e3948..2770a02 100644
--- a/wpilibc/src/main/native/include/frc/Counter.h
+++ b/wpilibc/src/main/native/include/frc/Counter.h
@@ -21,6 +21,8 @@
class DigitalGlitchFilter;
class SendableBuilder;
+class DMA;
+class DMASample;
/**
* Class for counting the number of ticks on a digital input channel.
@@ -36,6 +38,9 @@
public CounterBase,
public Sendable,
public SendableHelper<Counter> {
+ friend class DMA;
+ friend class DMASample;
+
public:
enum Mode {
kTwoPulse = 0,
diff --git a/wpilibc/src/main/native/include/frc/DMA.h b/wpilibc/src/main/native/include/frc/DMA.h
new file mode 100644
index 0000000..57cdd27
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/DMA.h
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <hal/Types.h>
+
+#include "frc/ErrorBase.h"
+
+namespace frc {
+class Encoder;
+class Counter;
+class DigitalSource;
+class DutyCycle;
+class AnalogInput;
+class DMASample;
+
+class DMA : public ErrorBase {
+ friend class DMASample;
+
+ public:
+ DMA();
+ ~DMA() override;
+
+ DMA& operator=(DMA&& other) = default;
+ DMA(DMA&& other) = default;
+
+ void SetPause(bool pause);
+ void SetRate(int cycles);
+
+ void AddEncoder(const Encoder* encoder);
+ void AddEncoderPeriod(const Encoder* encoder);
+
+ void AddCounter(const Counter* counter);
+ void AddCounterPeriod(const Counter* counter);
+
+ void AddDigitalSource(const DigitalSource* digitalSource);
+
+ void AddDutyCycle(const DutyCycle* digitalSource);
+
+ void AddAnalogInput(const AnalogInput* analogInput);
+ void AddAveragedAnalogInput(const AnalogInput* analogInput);
+ void AddAnalogAccumulator(const AnalogInput* analogInput);
+
+ void SetExternalTrigger(DigitalSource* source, bool rising, bool falling);
+
+ void StartDMA(int queueDepth);
+ void StopDMA();
+
+ private:
+ hal::Handle<HAL_DMAHandle> dmaHandle;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/DMASample.h b/wpilibc/src/main/native/include/frc/DMASample.h
new file mode 100644
index 0000000..4592930
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/DMASample.h
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <type_traits>
+
+#include <hal/AnalogInput.h>
+#include <hal/DMA.h>
+#include <units/units.h>
+
+#include "frc/AnalogInput.h"
+#include "frc/Counter.h"
+#include "frc/DMA.h"
+#include "frc/DutyCycle.h"
+#include "frc/Encoder.h"
+
+namespace frc {
+class DMASample : public HAL_DMASample {
+ public:
+ HAL_DMAReadStatus Update(const DMA* dma, units::second_t timeout,
+ int32_t* remaining, int32_t* status) {
+ units::millisecond_t ms = timeout;
+ auto timeoutMs = ms.to<int32_t>();
+ return HAL_ReadDMA(dma->dmaHandle, this, timeoutMs, remaining, status);
+ }
+
+ uint64_t GetTime() const { return timeStamp; }
+
+ units::second_t GetTimeStamp() const {
+ return units::second_t{static_cast<double>(GetTime()) * 1.0e-6};
+ }
+
+ int32_t GetEncoderRaw(const Encoder* encoder, int32_t* status) const {
+ return HAL_GetDMASampleEncoderRaw(this, encoder->m_encoder, status);
+ }
+
+ double GetEncoderDistance(const Encoder* encoder, int32_t* status) const {
+ double val = GetEncoderRaw(encoder, status);
+ val *= encoder->DecodingScaleFactor();
+ val *= encoder->GetDistancePerPulse();
+ return val;
+ }
+
+ int32_t GetEncoderPeriodRaw(const Encoder* encoder, int32_t* status) const {
+ return HAL_GetDMASampleEncoderPeriodRaw(this, encoder->m_encoder, status);
+ }
+
+ int32_t GetCounter(const Counter* counter, int32_t* status) const {
+ return HAL_GetDMASampleCounter(this, counter->m_counter, status);
+ }
+
+ int32_t GetCounterPeriod(const Counter* counter, int32_t* status) const {
+ return HAL_GetDMASampleCounterPeriod(this, counter->m_counter, status);
+ }
+
+ bool GetDigitalSource(const DigitalSource* digitalSource,
+ int32_t* status) const {
+ return HAL_GetDMASampleDigitalSource(
+ this, digitalSource->GetPortHandleForRouting(), status);
+ }
+
+ int32_t GetAnalogInputRaw(const AnalogInput* analogInput,
+ int32_t* status) const {
+ return HAL_GetDMASampleAnalogInputRaw(this, analogInput->m_port, status);
+ }
+
+ double GetAnalogInputVoltage(const AnalogInput* analogInput,
+ int32_t* status) {
+ return HAL_GetAnalogValueToVolts(
+ analogInput->m_port, GetAnalogInputRaw(analogInput, status), status);
+ }
+
+ int32_t GetAveragedAnalogInputRaw(const AnalogInput* analogInput,
+ int32_t* status) const {
+ return HAL_GetDMASampleAveragedAnalogInputRaw(this, analogInput->m_port,
+ status);
+ }
+
+ double GetAveragedAnalogInputVoltage(const AnalogInput* analogInput,
+ int32_t* status) {
+ return HAL_GetAnalogValueToVolts(
+ analogInput->m_port, GetAveragedAnalogInputRaw(analogInput, status),
+ status);
+ }
+
+ void GetAnalogAccumulator(const AnalogInput* analogInput, int64_t* count,
+ int64_t* value, int32_t* status) const {
+ return HAL_GetDMASampleAnalogAccumulator(this, analogInput->m_port, count,
+ value, status);
+ }
+
+ int32_t GetDutyCycleOutputRaw(const DutyCycle* dutyCycle,
+ int32_t* status) const {
+ return HAL_GetDMASampleDutyCycleOutputRaw(this, dutyCycle->m_handle,
+ status);
+ }
+
+ double GetDutyCycleOutput(const DutyCycle* dutyCycle, int32_t* status) {
+ return GetDutyCycleOutputRaw(dutyCycle, status) /
+ static_cast<double>(dutyCycle->GetOutputScaleFactor());
+ }
+};
+
+static_assert(std::is_standard_layout_v<frc::DMASample>,
+ "frc::DMASample must have standard layout");
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/DMC60.h b/wpilibc/src/main/native/include/frc/DMC60.h
index 5a33b5a..ecf01e1 100644
--- a/wpilibc/src/main/native/include/frc/DMC60.h
+++ b/wpilibc/src/main/native/include/frc/DMC60.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -13,6 +13,19 @@
/**
* Digilent DMC 60 Speed Controller.
+ *
+ * Note that the DMC 60 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.
+ *
+ * \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"
*/
class DMC60 : public PWMSpeedController {
public:
diff --git a/wpilibc/src/main/native/include/frc/DigitalOutput.h b/wpilibc/src/main/native/include/frc/DigitalOutput.h
index 45727a4..1d1d152 100644
--- a/wpilibc/src/main/native/include/frc/DigitalOutput.h
+++ b/wpilibc/src/main/native/include/frc/DigitalOutput.h
@@ -9,7 +9,7 @@
#include <hal/Types.h>
-#include "frc/ErrorBase.h"
+#include "frc/DigitalSource.h"
#include "frc/smartdashboard/Sendable.h"
#include "frc/smartdashboard/SendableHelper.h"
@@ -24,7 +24,7 @@
* elsewhere will allocate channels automatically so for those devices it
* shouldn't be done here.
*/
-class DigitalOutput : public ErrorBase,
+class DigitalOutput : public DigitalSource,
public Sendable,
public SendableHelper<DigitalOutput> {
public:
@@ -59,10 +59,26 @@
*/
bool Get() const;
+ // Digital Source Interface
+ /**
+ * @return The HAL Handle to the specified source.
+ */
+ HAL_Handle GetPortHandleForRouting() const override;
+
+ /**
+ * @return The type of analog trigger output to be used. 0 for Digitals
+ */
+ AnalogTriggerType GetAnalogTriggerTypeForRouting() const override;
+
+ /**
+ * Is source an AnalogTrigger
+ */
+ bool IsAnalogTrigger() const override;
+
/**
* @return The GPIO channel number that this object represents.
*/
- int GetChannel() const;
+ int GetChannel() const override;
/**
* Output a single pulse on the digital output line.
diff --git a/wpilibc/src/main/native/include/frc/DriverStation.h b/wpilibc/src/main/native/include/frc/DriverStation.h
index 8abffde..4508701 100644
--- a/wpilibc/src/main/native/include/frc/DriverStation.h
+++ b/wpilibc/src/main/native/include/frc/DriverStation.h
@@ -394,6 +394,11 @@
*/
void InTest(bool entering) { m_userInTest = entering; }
+ /**
+ * Forces WaitForData() to return immediately.
+ */
+ void WakeupWaitForData();
+
protected:
/**
* Copy data from the DS task for the user.
diff --git a/wpilibc/src/main/native/include/frc/DutyCycle.h b/wpilibc/src/main/native/include/frc/DutyCycle.h
new file mode 100644
index 0000000..f0fc2d8
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/DutyCycle.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Types.h>
+
+#include "frc/ErrorBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+class DigitalSource;
+class AnalogTrigger;
+class DMA;
+class DMASample;
+
+/**
+ * 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 DigitalSource.
+ *
+ * <p>These can be combined as the input of an AnalogTrigger to a Counter in
+ * order to implement rollover checking.
+ *
+ */
+class DutyCycle : public ErrorBase,
+ public Sendable,
+ public SendableHelper<DutyCycle> {
+ friend class AnalogTrigger;
+ friend class DMA;
+ friend class DMASample;
+
+ public:
+ /**
+ * Constructs a DutyCycle input from a DigitalSource input.
+ *
+ * <p> This class does not own the inputted source.
+ *
+ * @param source The DigitalSource to use.
+ */
+ explicit DutyCycle(DigitalSource& source);
+ /**
+ * Constructs a DutyCycle input from a DigitalSource input.
+ *
+ * <p> This class does not own the inputted source.
+ *
+ * @param source The DigitalSource to use.
+ */
+ explicit DutyCycle(DigitalSource* source);
+ /**
+ * Constructs a DutyCycle input from a DigitalSource input.
+ *
+ * <p> This class does not own the inputted source.
+ *
+ * @param source The DigitalSource to use.
+ */
+ explicit DutyCycle(std::shared_ptr<DigitalSource> source);
+
+ /**
+ * Close the DutyCycle and free all resources.
+ */
+ ~DutyCycle() override;
+
+ DutyCycle(DutyCycle&&) = default;
+ DutyCycle& operator=(DutyCycle&&) = default;
+
+ /**
+ * Get the frequency of the duty cycle signal.
+ *
+ * @return frequency in Hertz
+ */
+ int GetFrequency() const;
+
+ /**
+ * 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
+ */
+ double GetOutput() const;
+
+ /**
+ * 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
+ */
+ unsigned int GetOutputRaw() const;
+
+ /**
+ * 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
+ */
+ unsigned int GetOutputScaleFactor() const;
+
+ /**
+ * Get the FPGA index for the DutyCycle.
+ *
+ * @return the FPGA index
+ */
+ int GetFPGAIndex() const;
+
+ /**
+ * Get the channel of the source.
+ *
+ * @return the source channel
+ */
+ int GetSourceChannel() const;
+
+ protected:
+ void InitSendable(SendableBuilder& builder) override;
+
+ private:
+ void InitDutyCycle();
+ std::shared_ptr<DigitalSource> m_source;
+ hal::Handle<HAL_DutyCycleHandle> m_handle;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
new file mode 100644
index 0000000..92864a8
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
@@ -0,0 +1,174 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <hal/SimDevice.h>
+#include <hal/Types.h>
+#include <units/units.h>
+
+#include "frc/AnalogTrigger.h"
+#include "frc/Counter.h"
+#include "frc/ErrorBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+class DutyCycle;
+class DigitalSource;
+
+/**
+ * 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.
+ */
+class DutyCycleEncoder : public ErrorBase,
+ public Sendable,
+ public SendableHelper<DutyCycleEncoder> {
+ public:
+ /**
+ * Construct a new DutyCycleEncoder on a specific channel.
+ *
+ * @param channel the channel to attach to
+ */
+ explicit DutyCycleEncoder(int channel);
+
+ /**
+ * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+ *
+ * @param dutyCycle the duty cycle to attach to
+ */
+ explicit DutyCycleEncoder(DutyCycle& dutyCycle);
+
+ /**
+ * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+ *
+ * @param dutyCycle the duty cycle to attach to
+ */
+ explicit DutyCycleEncoder(DutyCycle* dutyCycle);
+
+ /**
+ * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+ *
+ * @param dutyCycle the duty cycle to attach to
+ */
+ explicit DutyCycleEncoder(std::shared_ptr<DutyCycle> dutyCycle);
+
+ /**
+ * Construct a new DutyCycleEncoder attached to a DigitalSource object.
+ *
+ * @param source the digital source to attach to
+ */
+ explicit DutyCycleEncoder(DigitalSource& digitalSource);
+
+ /**
+ * Construct a new DutyCycleEncoder attached to a DigitalSource object.
+ *
+ * @param source the digital source to attach to
+ */
+ explicit DutyCycleEncoder(DigitalSource* digitalSource);
+
+ /**
+ * Construct a new DutyCycleEncoder attached to a DigitalSource object.
+ *
+ * @param source the digital source to attach to
+ */
+ explicit DutyCycleEncoder(std::shared_ptr<DigitalSource> digitalSource);
+
+ ~DutyCycleEncoder() override = default;
+
+ DutyCycleEncoder(DutyCycleEncoder&&) = default;
+ DutyCycleEncoder& operator=(DutyCycleEncoder&&) = default;
+
+ /**
+ * Get the frequency in Hz of the duty cycle signal from the encoder.
+ *
+ * @return duty cycle frequency in Hz
+ */
+ int GetFrequency() const;
+
+ /**
+ * Get if the sensor is connected
+ *
+ * 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 SetConnectedFrequencyThreshold.
+ *
+ * @return true if the sensor is connected
+ */
+ bool IsConnected() const;
+
+ /**
+ * Change the frequency threshold for detecting connection used by
+ * IsConnected.
+ *
+ * @param frequency the minimum frequency in Hz.
+ */
+ void SetConnectedFrequencyThreshold(int frequency);
+
+ /**
+ * Reset the Encoder distance to zero.
+ */
+ void Reset();
+
+ /**
+ * Get the encoder value since the last reset.
+ *
+ * This is reported in rotations since the last reset.
+ *
+ * @return the encoder value in rotations
+ */
+ units::turn_t Get() const;
+
+ /**
+ * 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
+ */
+ void SetDistancePerRotation(double distancePerRotation);
+
+ /**
+ * Get the distance per rotation for this encoder.
+ *
+ * @return The scale factor that will be used to convert rotation to useful
+ * units.
+ */
+ double GetDistancePerRotation() const;
+
+ /**
+ * Get the distance the sensor has driven since the last reset as scaled by
+ * the value from SetDistancePerRotation.
+ *
+ * @return The distance driven since the last reset
+ */
+ double GetDistance() const;
+
+ void InitSendable(SendableBuilder& builder) override;
+
+ private:
+ void Init();
+
+ std::shared_ptr<DutyCycle> m_dutyCycle;
+ AnalogTrigger m_analogTrigger;
+ Counter m_counter;
+ int m_frequencyThreshold = 100;
+ double m_positionOffset = 0;
+ double m_distancePerRotation = 1.0;
+ mutable units::turn_t m_lastPosition{0.0};
+
+ hal::SimDevice m_simDevice;
+ hal::SimDouble m_simPosition;
+ hal::SimBoolean m_simIsConnected;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Encoder.h b/wpilibc/src/main/native/include/frc/Encoder.h
index 074cc5e..60552f6 100644
--- a/wpilibc/src/main/native/include/frc/Encoder.h
+++ b/wpilibc/src/main/native/include/frc/Encoder.h
@@ -23,6 +23,8 @@
class DigitalSource;
class DigitalGlitchFilter;
class SendableBuilder;
+class DMA;
+class DMASample;
/**
* Class to read quad encoders.
@@ -44,6 +46,9 @@
public PIDSource,
public Sendable,
public SendableHelper<Encoder> {
+ friend class DMA;
+ friend class DMASample;
+
public:
enum IndexingType {
kResetWhileHigh,
diff --git a/wpilibc/src/main/native/include/frc/ErrorBase.h b/wpilibc/src/main/native/include/frc/ErrorBase.h
index 7235352..e9168aa 100644
--- a/wpilibc/src/main/native/include/frc/ErrorBase.h
+++ b/wpilibc/src/main/native/include/frc/ErrorBase.h
@@ -16,6 +16,9 @@
#include "frc/Base.h"
#include "frc/Error.h"
+// Forward declared manually to avoid needing to pull in entire HAL header.
+extern "C" const char* HAL_GetErrorMessage(int32_t code);
+
#define wpi_setErrnoErrorWithContext(context) \
this->SetErrnoError((context), __FILE__, __FUNCTION__, __LINE__)
#define wpi_setErrnoError() wpi_setErrnoErrorWithContext("")
@@ -35,6 +38,22 @@
this->SetErrorRange((code), (min), (max), (req), (context), __FILE__, \
__FUNCTION__, __LINE__); \
} while (0)
+
+#define wpi_setHALError(code) \
+ do { \
+ if ((code) != 0) \
+ this->SetError((code), HAL_GetErrorMessage(code), __FILE__, \
+ __FUNCTION__, __LINE__); \
+ } while (0)
+
+#define wpi_setHALErrorWithRange(code, min, max, req) \
+ do { \
+ if ((code) != 0) \
+ this->SetErrorRange((code), (min), (max), (req), \
+ HAL_GetErrorMessage(code), __FILE__, __FUNCTION__, \
+ __LINE__); \
+ } while (0)
+
#define wpi_setError(code) wpi_setErrorWithContext(code, "")
#define wpi_setStaticErrorWithContext(object, code, context) \
do { \
@@ -43,12 +62,21 @@
} while (0)
#define wpi_setStaticError(object, code) \
wpi_setStaticErrorWithContext(object, code, "")
+
#define wpi_setGlobalErrorWithContext(code, context) \
do { \
if ((code) != 0) \
::frc::ErrorBase::SetGlobalError((code), (context), __FILE__, \
__FUNCTION__, __LINE__); \
} while (0)
+
+#define wpi_setGlobalHALError(code) \
+ do { \
+ if ((code) != 0) \
+ ::frc::ErrorBase::SetGlobalError((code), HAL_GetErrorMessage(code), \
+ __FILE__, __FUNCTION__, __LINE__); \
+ } while (0)
+
#define wpi_setGlobalError(code) wpi_setGlobalErrorWithContext(code, "")
#define wpi_setWPIErrorWithContext(error, context) \
this->SetWPIError(wpi_error_s_##error(), wpi_error_value_##error(), \
diff --git a/wpilibc/src/main/native/include/frc/IterativeRobot.h b/wpilibc/src/main/native/include/frc/IterativeRobot.h
index 8ce6356..447c477 100644
--- a/wpilibc/src/main/native/include/frc/IterativeRobot.h
+++ b/wpilibc/src/main/native/include/frc/IterativeRobot.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -7,6 +7,8 @@
#pragma once
+#include <atomic>
+
#include "frc/IterativeRobotBase.h"
namespace frc {
@@ -28,9 +30,6 @@
IterativeRobot();
virtual ~IterativeRobot() = default;
- IterativeRobot(IterativeRobot&&) = default;
- IterativeRobot& operator=(IterativeRobot&&) = default;
-
/**
* Provide an alternate "main loop" via StartCompetition().
*
@@ -38,6 +37,14 @@
* with the DS packets.
*/
void StartCompetition() override;
+
+ /**
+ * Ends the main loop in StartCompetition().
+ */
+ void EndCompetition() override;
+
+ private:
+ std::atomic<bool> m_exit{false};
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Jaguar.h b/wpilibc/src/main/native/include/frc/Jaguar.h
index f22eb42..7a8503e 100644
--- a/wpilibc/src/main/native/include/frc/Jaguar.h
+++ b/wpilibc/src/main/native/include/frc/Jaguar.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -13,6 +13,19 @@
/**
* Luminary Micro / Vex Robotics Jaguar Speed Controller with PWM control.
+ *
+ * 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.
+ *
+ * \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"
*/
class Jaguar : public PWMSpeedController {
public:
diff --git a/wpilibc/src/main/native/include/frc/LinearFilter.h b/wpilibc/src/main/native/include/frc/LinearFilter.h
index de52003..2b83a32 100644
--- a/wpilibc/src/main/native/include/frc/LinearFilter.h
+++ b/wpilibc/src/main/native/include/frc/LinearFilter.h
@@ -7,9 +7,12 @@
#pragma once
+#include <cassert>
+#include <cmath>
#include <initializer_list>
#include <vector>
+#include <hal/FRCUsageReporting.h>
#include <units/units.h>
#include <wpi/ArrayRef.h>
#include <wpi/circular_buffer.h>
@@ -66,6 +69,7 @@
* Combining this with Note 1 - the impetus is on YOU as a developer to make
* sure Calculate() gets called at the desired, constant frequency!
*/
+template <class T>
class LinearFilter {
public:
/**
@@ -74,7 +78,15 @@
* @param ffGains The "feed forward" or FIR gains.
* @param fbGains The "feed back" or IIR gains.
*/
- LinearFilter(wpi::ArrayRef<double> ffGains, wpi::ArrayRef<double> fbGains);
+ LinearFilter(wpi::ArrayRef<double> ffGains, wpi::ArrayRef<double> fbGains)
+ : m_inputs(ffGains.size()),
+ m_outputs(fbGains.size()),
+ m_inputGains(ffGains),
+ m_outputGains(fbGains) {
+ static int instances = 0;
+ instances++;
+ HAL_Report(HALUsageReporting::kResourceType_LinearFilter, instances);
+ }
/**
* Create a linear FIR or IIR filter.
@@ -99,8 +111,11 @@
* @param period The period in seconds between samples taken by the
* user.
*/
- static LinearFilter SinglePoleIIR(double timeConstant,
- units::second_t period);
+ static LinearFilter<T> SinglePoleIIR(double timeConstant,
+ units::second_t period) {
+ double gain = std::exp(-period.to<double>() / timeConstant);
+ return LinearFilter(1.0 - gain, -gain);
+ }
/**
* Creates a first-order high-pass filter of the form:<br>
@@ -113,7 +128,10 @@
* @param period The period in seconds between samples taken by the
* user.
*/
- static LinearFilter HighPass(double timeConstant, units::second_t period);
+ static LinearFilter<T> HighPass(double timeConstant, units::second_t period) {
+ double gain = std::exp(-period.to<double>() / timeConstant);
+ return LinearFilter({gain, -gain}, {-gain});
+ }
/**
* Creates a K-tap FIR moving average filter of the form:<br>
@@ -124,12 +142,20 @@
* @param taps The number of samples to average over. Higher = smoother but
* slower
*/
- static LinearFilter MovingAverage(int taps);
+ static LinearFilter<T> MovingAverage(int taps) {
+ assert(taps > 0);
+
+ std::vector<double> gains(taps, 1.0 / taps);
+ return LinearFilter(gains, {});
+ }
/**
* Reset the filter state.
*/
- void Reset();
+ void Reset() {
+ m_inputs.reset();
+ m_outputs.reset();
+ }
/**
* Calculates the next value of the filter.
@@ -138,11 +164,29 @@
*
* @return The filtered value at this step
*/
- double Calculate(double input);
+ T Calculate(T input) {
+ T retVal = T(0.0);
+
+ // Rotate the inputs
+ m_inputs.push_front(input);
+
+ // Calculate the new value
+ for (size_t i = 0; i < m_inputGains.size(); i++) {
+ retVal += m_inputs[i] * m_inputGains[i];
+ }
+ for (size_t i = 0; i < m_outputGains.size(); i++) {
+ retVal -= m_outputs[i] * m_outputGains[i];
+ }
+
+ // Rotate the outputs
+ m_outputs.push_front(retVal);
+
+ return retVal;
+ }
private:
- wpi::circular_buffer<double> m_inputs{0};
- wpi::circular_buffer<double> m_outputs{0};
+ wpi::circular_buffer<T> m_inputs;
+ wpi::circular_buffer<T> m_outputs;
std::vector<double> m_inputGains;
std::vector<double> m_outputGains;
};
diff --git a/wpilibc/src/main/native/include/frc/MedianFilter.h b/wpilibc/src/main/native/include/frc/MedianFilter.h
new file mode 100644
index 0000000..b5d499b
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/MedianFilter.h
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <algorithm>
+#include <vector>
+
+#include <wpi/Algorithm.h>
+#include <wpi/circular_buffer.h>
+
+namespace frc {
+/**
+ * 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).
+ */
+template <class T>
+class MedianFilter {
+ public:
+ /**
+ * Creates a new MedianFilter.
+ *
+ * @param size The number of samples in the moving window.
+ */
+ explicit MedianFilter(size_t size) : m_valueBuffer(size), 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.
+ */
+ T Calculate(T next) {
+ // Insert next value at proper point in sorted array
+ wpi::insert_sorted(m_orderedValues, next);
+
+ size_t 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.erase(std::find(m_orderedValues.begin(),
+ m_orderedValues.end(),
+ m_valueBuffer.pop_back()));
+ curSize = curSize - 1;
+ }
+
+ // Add next value to circular buffer
+ m_valueBuffer.push_front(next);
+
+ if (curSize % 2 == 1) {
+ // If size is odd, return middle element of sorted list
+ return m_orderedValues[curSize / 2];
+ } else {
+ // If size is even, return average of middle elements
+ return (m_orderedValues[curSize / 2 - 1] + m_orderedValues[curSize / 2]) /
+ 2.0;
+ }
+ }
+
+ /**
+ * Resets the filter, clearing the window of all elements.
+ */
+ void Reset() {
+ m_orderedValues.clear();
+ m_valueBuffer.reset();
+ }
+
+ private:
+ wpi::circular_buffer<T> m_valueBuffer;
+ std::vector<T> m_orderedValues;
+ size_t m_size;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Notifier.h b/wpilibc/src/main/native/include/frc/Notifier.h
index f2f37f1..24ffba3 100644
--- a/wpilibc/src/main/native/include/frc/Notifier.h
+++ b/wpilibc/src/main/native/include/frc/Notifier.h
@@ -16,6 +16,7 @@
#include <hal/Types.h>
#include <units/units.h>
+#include <wpi/Twine.h>
#include <wpi/deprecated.h>
#include <wpi/mutex.h>
@@ -47,6 +48,13 @@
Notifier& operator=(Notifier&& rhs);
/**
+ * Sets the name of the notifier. Used for debugging purposes only.
+ *
+ * @param name Name
+ */
+ void SetName(const wpi::Twine& name);
+
+ /**
* Change the handler function.
*
* @param handler Handler
diff --git a/wpilibc/src/main/native/include/frc/PIDBase.h b/wpilibc/src/main/native/include/frc/PIDBase.h
index 098718f..513d46b 100644
--- a/wpilibc/src/main/native/include/frc/PIDBase.h
+++ b/wpilibc/src/main/native/include/frc/PIDBase.h
@@ -402,7 +402,7 @@
double m_error = 0;
double m_result = 0;
- LinearFilter m_filter{{}, {}};
+ LinearFilter<double> m_filter{{}, {}};
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/PWM.h b/wpilibc/src/main/native/include/frc/PWM.h
index 58e18d2..406a93e 100644
--- a/wpilibc/src/main/native/include/frc/PWM.h
+++ b/wpilibc/src/main/native/include/frc/PWM.h
@@ -17,7 +17,7 @@
#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
-
+class AddressableLED;
class SendableBuilder;
/**
@@ -34,11 +34,12 @@
* - 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)
+ * - 1 = minimum pulse width (currently 0.5ms)
* - 0 = disabled (i.e. PWM output is held low)
*/
class PWM : public MotorSafety, public Sendable, public SendableHelper<PWM> {
public:
+ friend class AddressableLED;
/**
* Represents the amount to multiply the minimum servo-pulse pwm period by.
*/
diff --git a/wpilibc/src/main/native/include/frc/PWMSparkMax.h b/wpilibc/src/main/native/include/frc/PWMSparkMax.h
index c8b22d7..3ce6466 100644
--- a/wpilibc/src/main/native/include/frc/PWMSparkMax.h
+++ b/wpilibc/src/main/native/include/frc/PWMSparkMax.h
@@ -12,14 +12,27 @@
namespace frc {
/**
- * REV Robotics SparkMax Speed Controller.
+ * REV Robotics SPARK MAX Speed Controller.
+ *
+ * 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.
+ *
+ * \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"
*/
class PWMSparkMax : public PWMSpeedController {
public:
/**
- * Constructor for a SparkMax.
+ * Constructor for a SPARK MAX.
*
- * @param channel The PWM channel that the Spark is attached to. 0-9 are
+ * @param channel The PWM channel that the SPARK MAX is attached to. 0-9 are
* on-board, 10-19 are on the MXP port
*/
explicit PWMSparkMax(int channel);
diff --git a/wpilibc/src/main/native/include/frc/PWMTalonSRX.h b/wpilibc/src/main/native/include/frc/PWMTalonSRX.h
index 9260e1e..b9c8369 100644
--- a/wpilibc/src/main/native/include/frc/PWMTalonSRX.h
+++ b/wpilibc/src/main/native/include/frc/PWMTalonSRX.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -14,13 +14,26 @@
/**
* Cross the Road Electronics (CTRE) Talon SRX Speed Controller with PWM
* control.
+ *
+ * Note that the Talon SRX 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 SRX User
+ * Manual available from Cross The Road Electronics.
+ *
+ * \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"
*/
class PWMTalonSRX : public PWMSpeedController {
public:
/**
- * Construct a PWMTalonSRX connected via PWM.
+ * Construct a Talon SRX connected via PWM.
*
- * @param channel The PWM channel that the PWMTalonSRX is attached to. 0-9 are
+ * @param channel The PWM channel that the Talon SRX is attached to. 0-9 are
* on-board, 10-19 are on the MXP port
*/
explicit PWMTalonSRX(int channel);
diff --git a/wpilibc/src/main/native/include/frc/PWMVictorSPX.h b/wpilibc/src/main/native/include/frc/PWMVictorSPX.h
index d7112c9..a19e704 100644
--- a/wpilibc/src/main/native/include/frc/PWMVictorSPX.h
+++ b/wpilibc/src/main/native/include/frc/PWMVictorSPX.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -14,13 +14,26 @@
/**
* Cross the Road Electronics (CTRE) Victor SPX Speed Controller with PWM
* control.
+ *
+ * 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 Cross The Road Electronics.
+ *
+ * \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"
*/
class PWMVictorSPX : public PWMSpeedController {
public:
/**
- * Construct a PWMVictorSPX connected via PWM.
+ * Construct a Victor SPX connected via PWM.
*
- * @param channel The PWM channel that the PWMVictorSPX is attached to. 0-9
+ * @param channel The PWM channel that the Victor SPX is attached to. 0-9
* are on-board, 10-19 are on the MXP port
*/
explicit PWMVictorSPX(int channel);
diff --git a/wpilibc/src/main/native/include/frc/RobotBase.h b/wpilibc/src/main/native/include/frc/RobotBase.h
index 85a9d12..725aa97 100644
--- a/wpilibc/src/main/native/include/frc/RobotBase.h
+++ b/wpilibc/src/main/native/include/frc/RobotBase.h
@@ -7,9 +7,12 @@
#pragma once
+#include <chrono>
#include <thread>
#include <hal/Main.h>
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
#include <wpi/raw_ostream.h>
#include "frc/Base.h"
@@ -23,9 +26,13 @@
namespace impl {
template <class Robot>
-void RunRobot() {
- static Robot robot;
- robot.StartCompetition();
+void RunRobot(wpi::mutex& m, Robot** robot) {
+ static Robot theRobot;
+ {
+ std::scoped_lock lock{m};
+ *robot = &theRobot;
+ }
+ theRobot.StartCompetition();
}
} // namespace impl
@@ -36,20 +43,50 @@
if (halInit != 0) {
return halInit;
}
+
+ static wpi::mutex m;
+ static wpi::condition_variable cv;
+ static Robot* robot = nullptr;
+ static bool exited = false;
+
if (HAL_HasMain()) {
- std::thread([] {
+ std::thread thr([] {
try {
- impl::RunRobot<Robot>();
+ impl::RunRobot<Robot>(m, &robot);
} catch (...) {
HAL_ExitMain();
+ {
+ std::scoped_lock lock{m};
+ robot = nullptr;
+ exited = true;
+ }
+ cv.notify_all();
throw;
}
+
HAL_ExitMain();
- })
- .detach();
+ {
+ std::scoped_lock lock{m};
+ robot = nullptr;
+ exited = true;
+ }
+ cv.notify_all();
+ });
+
HAL_RunMain();
+
+ // signal loop to exit
+ if (robot) robot->EndCompetition();
+
+ // prefer to join, but detach to exit if it doesn't exit in a timely manner
+ using namespace std::chrono_literals;
+ std::unique_lock lock{m};
+ if (cv.wait_for(lock, 1s, [] { return exited; }))
+ thr.join();
+ else
+ thr.detach();
} else {
- impl::RunRobot<Robot>();
+ impl::RunRobot<Robot>(m, &robot);
}
return 0;
@@ -127,6 +164,8 @@
virtual void StartCompetition() = 0;
+ virtual void EndCompetition() = 0;
+
static constexpr bool IsReal() {
#ifdef __FRC_ROBORIO__
return true;
diff --git a/wpilibc/src/main/native/include/frc/SD540.h b/wpilibc/src/main/native/include/frc/SD540.h
index 91666b4..07f7f18 100644
--- a/wpilibc/src/main/native/include/frc/SD540.h
+++ b/wpilibc/src/main/native/include/frc/SD540.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -13,6 +13,19 @@
/**
* Mindsensors SD540 Speed Controller.
+ *
+ * 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.
+ *
+ * \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"
*/
class SD540 : public PWMSpeedController {
public:
diff --git a/wpilibc/src/main/native/include/frc/Servo.h b/wpilibc/src/main/native/include/frc/Servo.h
index ae5c599..16b4281 100644
--- a/wpilibc/src/main/native/include/frc/Servo.h
+++ b/wpilibc/src/main/native/include/frc/Servo.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -105,7 +105,7 @@
static constexpr double kMinServoAngle = 0.0;
static constexpr double kDefaultMaxServoPWM = 2.4;
- static constexpr double kDefaultMinServoPWM = .6;
+ static constexpr double kDefaultMinServoPWM = 0.6;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Spark.h b/wpilibc/src/main/native/include/frc/Spark.h
index 640696f..24ed8f5 100644
--- a/wpilibc/src/main/native/include/frc/Spark.h
+++ b/wpilibc/src/main/native/include/frc/Spark.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -12,14 +12,27 @@
namespace frc {
/**
- * REV Robotics Speed Controller.
+ * REV Robotics SPARK Speed Controller.
+ *
+ * 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.
+ *
+ * \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"
*/
class Spark : public PWMSpeedController {
public:
/**
- * Constructor for a Spark.
+ * Constructor for a SPARK.
*
- * @param channel The PWM channel that the Spark is attached to. 0-9 are
+ * @param channel The PWM channel that the SPARK is attached to. 0-9 are
* on-board, 10-19 are on the MXP port
*/
explicit Spark(int channel);
diff --git a/wpilibc/src/main/native/include/frc/SpeedController.h b/wpilibc/src/main/native/include/frc/SpeedController.h
index 49a828b..2b11aee 100644
--- a/wpilibc/src/main/native/include/frc/SpeedController.h
+++ b/wpilibc/src/main/native/include/frc/SpeedController.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -7,6 +7,8 @@
#pragma once
+#include <units/units.h>
+
#include "frc/PIDOutput.h"
namespace frc {
@@ -26,6 +28,20 @@
virtual void Set(double speed) = 0;
/**
+ * 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 output The voltage to output.
+ */
+ virtual void SetVoltage(units::volt_t output);
+
+ /**
* 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.
diff --git a/wpilibc/src/main/native/include/frc/Talon.h b/wpilibc/src/main/native/include/frc/Talon.h
index 9a5400f..6e92dfc 100644
--- a/wpilibc/src/main/native/include/frc/Talon.h
+++ b/wpilibc/src/main/native/include/frc/Talon.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -13,6 +13,19 @@
/**
* Cross the Road Electronics (CTRE) Talon and Talon SR Speed Controller.
+ *
+ * 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.
+ *
+ * \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"
*/
class Talon : public PWMSpeedController {
public:
diff --git a/wpilibc/src/main/native/include/frc/TimedRobot.h b/wpilibc/src/main/native/include/frc/TimedRobot.h
index 1c8ff80..112e2c9 100644
--- a/wpilibc/src/main/native/include/frc/TimedRobot.h
+++ b/wpilibc/src/main/native/include/frc/TimedRobot.h
@@ -35,6 +35,11 @@
void StartCompetition() override;
/**
+ * Ends the main loop in StartCompetition().
+ */
+ void EndCompetition() override;
+
+ /**
* Get the time period between calls to Periodic() functions.
*/
units::second_t GetPeriod() const;
diff --git a/wpilibc/src/main/native/include/frc/Victor.h b/wpilibc/src/main/native/include/frc/Victor.h
index 5b6cba4..89c4a89 100644
--- a/wpilibc/src/main/native/include/frc/Victor.h
+++ b/wpilibc/src/main/native/include/frc/Victor.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -16,6 +16,20 @@
*
* 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.
+ *
+ * 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 as well 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 884 User
+ * Manual available from Vex.
+ *
+ * \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"
*/
class Victor : public PWMSpeedController {
public:
diff --git a/wpilibc/src/main/native/include/frc/VictorSP.h b/wpilibc/src/main/native/include/frc/VictorSP.h
index b23fba1..d8aa8e8 100644
--- a/wpilibc/src/main/native/include/frc/VictorSP.h
+++ b/wpilibc/src/main/native/include/frc/VictorSP.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -13,11 +13,24 @@
/**
* Vex Robotics Victor SP Speed Controller.
+ *
+ * Note that the Victor SP 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 SP User
+ * Manual available from Vex.
+ *
+ * \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"
*/
class VictorSP : public PWMSpeedController {
public:
/**
- * Constructor for a VictorSP.
+ * Constructor for a Victor SP.
*
* @param channel The PWM channel that the VictorSP is attached to. 0-9 are
* on-board, 10-19 are on the MXP port
diff --git a/wpilibc/src/main/native/include/frc/WPILib.h b/wpilibc/src/main/native/include/frc/WPILib.h
index 69fa061..9d62514 100644
--- a/wpilibc/src/main/native/include/frc/WPILib.h
+++ b/wpilibc/src/main/native/include/frc/WPILib.h
@@ -16,8 +16,10 @@
// clang-format on
+#if __has_include(<cameraserver/CameraServer.h>)
#include <cameraserver/CameraServer.h>
#include <vision/VisionRunner.h>
+#endif
#include "frc/ADXL345_I2C.h"
#include "frc/ADXL345_SPI.h"
@@ -82,6 +84,7 @@
#include "frc/VictorSP.h"
#include "frc/WPIErrors.h"
#include "frc/XboxController.h"
+#if __has_include("frc/buttons/InternalButton.h")
#include "frc/buttons/InternalButton.h"
#include "frc/buttons/JoystickButton.h"
#include "frc/buttons/NetworkButton.h"
@@ -90,12 +93,12 @@
#include "frc/commands/PIDCommand.h"
#include "frc/commands/PIDSubsystem.h"
#include "frc/commands/PrintCommand.h"
-#include "frc/commands/Scheduler.h"
#include "frc/commands/StartCommand.h"
#include "frc/commands/Subsystem.h"
#include "frc/commands/WaitCommand.h"
#include "frc/commands/WaitForChildren.h"
#include "frc/commands/WaitUntilCommand.h"
+#endif
#include "frc/drive/DifferentialDrive.h"
#include "frc/drive/KilloughDrive.h"
#include "frc/drive/MecanumDrive.h"
diff --git a/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h b/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h
new file mode 100644
index 0000000..6f6e086
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h
@@ -0,0 +1,149 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/MathExtras.h>
+
+namespace frc {
+/**
+ * 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).
+ */
+class ArmFeedforward {
+ using Angle = units::radians;
+ using Velocity = units::radians_per_second;
+ using Acceleration = units::compound_unit<units::radians_per_second,
+ units::inverse<units::second>>;
+ using kv_unit =
+ units::compound_unit<units::volts,
+ units::inverse<units::radians_per_second>>;
+ using ka_unit =
+ units::compound_unit<units::volts, units::inverse<Acceleration>>;
+
+ public:
+ constexpr ArmFeedforward() = default;
+
+ /**
+ * Creates a new ArmFeedforward with the specified gains.
+ *
+ * @param kS The static gain, in volts.
+ * @param kCos The gravity gain, in volts.
+ * @param kV The velocity gain, in volt seconds per radian.
+ * @param kA The acceleration gain, in volt seconds^2 per radian.
+ */
+ constexpr ArmFeedforward(
+ units::volt_t kS, units::volt_t kCos, units::unit_t<kv_unit> kV,
+ units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
+ : kS(kS), kCos(kCos), kV(kV), kA(kA) {}
+
+ /**
+ * Calculates the feedforward from the gains and setpoints.
+ *
+ * @param angle The angle setpoint, in radians.
+ * @param velocity The velocity setpoint, in radians per second.
+ * @param acceleration The acceleration setpoint, in radians per second^2.
+ * @return The computed feedforward, in volts.
+ */
+ units::volt_t Calculate(units::unit_t<Angle> angle,
+ units::unit_t<Velocity> velocity,
+ units::unit_t<Acceleration> acceleration =
+ units::unit_t<Acceleration>(0)) const {
+ return kS * wpi::sgn(velocity) + kCos * units::math::cos(angle) +
+ kV * velocity + kA * acceleration;
+ }
+
+ // 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.
+ */
+ units::unit_t<Velocity> MaxAchievableVelocity(
+ units::volt_t maxVoltage, units::unit_t<Angle> angle,
+ units::unit_t<Acceleration> acceleration) {
+ // Assume max velocity is positive
+ return (maxVoltage - kS - kCos * units::math::cos(angle) -
+ kA * acceleration) /
+ 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.
+ */
+ units::unit_t<Velocity> MinAchievableVelocity(
+ units::volt_t maxVoltage, units::unit_t<Angle> angle,
+ units::unit_t<Acceleration> acceleration) {
+ // Assume min velocity is negative, ks flips sign
+ return (-maxVoltage + kS - kCos * units::math::cos(angle) -
+ kA * acceleration) /
+ 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 and angle.
+ */
+ units::unit_t<Acceleration> MaxAchievableAcceleration(
+ units::volt_t maxVoltage, units::unit_t<Angle> angle,
+ units::unit_t<Velocity> velocity) {
+ return (maxVoltage - kS * wpi::sgn(velocity) -
+ kCos * units::math::cos(angle) - kV * velocity) /
+ 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 and angle.
+ */
+ units::unit_t<Acceleration> MinAchievableAcceleration(
+ units::volt_t maxVoltage, units::unit_t<Angle> angle,
+ units::unit_t<Velocity> velocity) {
+ return MaxAchievableAcceleration(-maxVoltage, angle, velocity);
+ }
+
+ units::volt_t kS{0};
+ units::volt_t kCos{0};
+ units::unit_t<kv_unit> kV{0};
+ units::unit_t<ka_unit> kA{0};
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h
new file mode 100644
index 0000000..c664fc4
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h
@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/MathExtras.h>
+
+namespace frc {
+/**
+ * A helper class that computes feedforward outputs for a simple elevator
+ * (modeled as a motor acting against the force of gravity).
+ */
+template <class Distance>
+class ElevatorFeedforward {
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Acceleration =
+ units::compound_unit<Velocity, units::inverse<units::seconds>>;
+ using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
+ using ka_unit =
+ units::compound_unit<units::volts, units::inverse<Acceleration>>;
+
+ public:
+ ElevatorFeedforward() = default;
+
+ /**
+ * Creates a new ElevatorFeedforward with the specified gains.
+ *
+ * @param kS The static gain, in volts.
+ * @param kG The gravity gain, in volts.
+ * @param kV The velocity gain, in volt seconds per distance.
+ * @param kA The acceleration gain, in volt seconds^2 per distance.
+ */
+ constexpr ElevatorFeedforward(
+ units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
+ units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
+ : kS(kS), kG(kG), kV(kV), kA(kA) {}
+
+ /**
+ * Calculates the feedforward from the gains and setpoints.
+ *
+ * @param velocity The velocity setpoint, in distance per second.
+ * @param acceleration The acceleration setpoint, in distance per second^2.
+ * @return The computed feedforward, in volts.
+ */
+ constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
+ units::unit_t<Acceleration> acceleration =
+ units::unit_t<Acceleration>(0)) {
+ return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
+ }
+
+ // 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.
+ */
+ constexpr units::unit_t<Velocity> MaxAchievableVelocity(
+ units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+ // Assume max velocity is positive
+ return (maxVoltage - kS - kG - kA * acceleration) / 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.
+ */
+ constexpr units::unit_t<Velocity> MinAchievableVelocity(
+ units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+ // Assume min velocity is negative, ks flips sign
+ return (-maxVoltage + kS - kG - kA * acceleration) / 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.
+ */
+ constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
+ units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+ return (maxVoltage - kS * wpi::sgn(velocity) - kG - kV * velocity) / 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.
+ */
+ constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
+ units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+ return MaxAchievableAcceleration(-maxVoltage, velocity);
+ }
+
+ units::volt_t kS{0};
+ units::volt_t kG{0};
+ units::unit_t<kv_unit> kV{0};
+ units::unit_t<ka_unit> kA{0};
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
index 64da9eb..96fd331 100644
--- a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -7,6 +7,8 @@
#pragma once
+#include <algorithm>
+#include <cmath>
#include <functional>
#include <limits>
@@ -14,6 +16,7 @@
#include "frc/controller/PIDController.h"
#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableBuilder.h"
#include "frc/smartdashboard/SendableHelper.h"
#include "frc/trajectory/TrapezoidProfile.h"
@@ -23,8 +26,20 @@
* Implements a PID control loop whose setpoint is constrained by a trapezoid
* profile.
*/
-class ProfiledPIDController : public Sendable,
- public SendableHelper<ProfiledPIDController> {
+template <class Distance>
+class ProfiledPIDController
+ : public Sendable,
+ public SendableHelper<ProfiledPIDController<Distance>> {
+ using Distance_t = units::unit_t<Distance>;
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Velocity_t = units::unit_t<Velocity>;
+ using Acceleration =
+ units::compound_unit<Velocity, units::inverse<units::seconds>>;
+ using Acceleration_t = units::unit_t<Acceleration>;
+ using State = typename TrapezoidProfile<Distance>::State;
+ using Constraints = typename TrapezoidProfile<Distance>::Constraints;
+
public:
/**
* Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
@@ -38,8 +53,8 @@
* default is 20 milliseconds.
*/
ProfiledPIDController(double Kp, double Ki, double Kd,
- frc::TrapezoidProfile::Constraints constraints,
- units::second_t period = 20_ms);
+ Constraints constraints, units::second_t period = 20_ms)
+ : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}
~ProfiledPIDController() override = default;
@@ -57,96 +72,98 @@
* @param Ki Integral coefficient
* @param Kd Differential coefficient
*/
- void SetPID(double Kp, double Ki, double Kd);
+ 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
*/
- void SetP(double Kp);
+ void SetP(double Kp) { m_controller.SetP(Kp); }
/**
* Sets the integral coefficient of the PID controller gain.
*
* @param Ki integral coefficient
*/
- void SetI(double Ki);
+ void SetI(double Ki) { m_controller.SetI(Ki); }
/**
* Sets the differential coefficient of the PID controller gain.
*
* @param Kd differential coefficient
*/
- void SetD(double Kd);
+ void SetD(double Kd) { m_controller.SetD(Kd); }
/**
* Gets the proportional coefficient.
*
* @return proportional coefficient
*/
- double GetP() const;
+ double GetP() const { return m_controller.GetP(); }
/**
* Gets the integral coefficient.
*
* @return integral coefficient
*/
- double GetI() const;
+ double GetI() const { return m_controller.GetI(); }
/**
* Gets the differential coefficient.
*
* @return differential coefficient
*/
- double GetD() const;
+ double GetD() const { return m_controller.GetD(); }
/**
* Gets the period of this controller.
*
* @return The period of the controller.
*/
- units::second_t GetPeriod() const;
+ units::second_t GetPeriod() const { return m_controller.GetPeriod(); }
/**
* Sets the goal for the ProfiledPIDController.
*
* @param goal The desired unprofiled setpoint.
*/
- void SetGoal(TrapezoidProfile::State goal);
+ void SetGoal(State goal) { m_goal = goal; }
/**
* Sets the goal for the ProfiledPIDController.
*
* @param goal The desired unprofiled setpoint.
*/
- void SetGoal(units::meter_t goal);
+ void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t(0)}; }
/**
* Gets the goal for the ProfiledPIDController.
*/
- TrapezoidProfile::State GetGoal() const;
+ State GetGoal() const { return m_goal; }
/**
* Returns true if the error is within the tolerance of the error.
*
* This will return false until at least one input value has been computed.
*/
- bool AtGoal() const;
+ bool AtGoal() const { return AtSetpoint() && m_goal == m_setpoint; }
/**
* Set velocity and acceleration constraints for goal.
*
* @param constraints Velocity and acceleration constraints for goal.
*/
- void SetConstraints(frc::TrapezoidProfile::Constraints constraints);
+ void SetConstraints(Constraints constraints) { m_constraints = constraints; }
/**
* Returns the current setpoint of the ProfiledPIDController.
*
* @return The current setpoint.
*/
- TrapezoidProfile::State GetSetpoint() const;
+ State GetSetpoint() const { return m_setpoint; }
/**
* Returns true if the error is within the tolerance of the error.
@@ -157,7 +174,7 @@
*
* This will return false until at least one input value has been computed.
*/
- bool AtSetpoint() const;
+ bool AtSetpoint() const { return m_controller.AtSetpoint(); }
/**
* Enables continuous input.
@@ -169,12 +186,15 @@
* @param minimumInput The minimum value expected from the input.
* @param maximumInput The maximum value expected from the input.
*/
- void EnableContinuousInput(double minimumInput, double maximumInput);
+ void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
+ m_controller.EnableContinuousInput(minimumInput.template to<double>(),
+ maximumInput.template to<double>());
+ }
/**
* Disables continuous input.
*/
- void DisableContinuousInput();
+ void DisableContinuousInput() { m_controller.DisableContinuousInput(); }
/**
* Sets the minimum and maximum values for the integrator.
@@ -185,7 +205,9 @@
* @param minimumIntegral The minimum value of the integrator.
* @param maximumIntegral The maximum value of the integrator.
*/
- void SetIntegratorRange(double minimumIntegral, double maximumIntegral);
+ void SetIntegratorRange(double minimumIntegral, double maximumIntegral) {
+ m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
+ }
/**
* Sets the error which is considered tolerable for use with
@@ -195,27 +217,39 @@
* @param velocityTolerance Velocity error which is tolerable.
*/
void SetTolerance(
- double positionTolerance,
- double velocityTolerance = std::numeric_limits<double>::infinity());
+ Distance_t positionTolerance,
+ Velocity_t velocityTolerance = std::numeric_limits<double>::infinity()) {
+ m_controller.SetTolerance(positionTolerance.template to<double>(),
+ velocityTolerance.template to<double>());
+ }
/**
* Returns the difference between the setpoint and the measurement.
*
* @return The error.
*/
- double GetPositionError() const;
+ Distance_t GetPositionError() const {
+ return Distance_t(m_controller.GetPositionError());
+ }
/**
* Returns the change in error per second.
*/
- double GetVelocityError() const;
+ Velocity_t GetVelocityError() const {
+ return Velocity_t(m_controller.GetVelocityError());
+ }
/**
* Returns the next output of the PID controller.
*
* @param measurement The current measurement of the process variable.
*/
- double Calculate(units::meter_t measurement);
+ double Calculate(Distance_t measurement) {
+ frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
+ m_setpoint = profile.Calculate(GetPeriod());
+ return m_controller.Calculate(measurement.template to<double>(),
+ m_setpoint.position.template to<double>());
+ }
/**
* Returns the next output of the PID controller.
@@ -223,15 +257,20 @@
* @param measurement The current measurement of the process variable.
* @param goal The new goal of the controller.
*/
- double Calculate(units::meter_t measurement, TrapezoidProfile::State goal);
-
+ double Calculate(Distance_t measurement, State 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.
*/
- double Calculate(units::meter_t measurement, units::meter_t goal);
+ double Calculate(Distance_t measurement, Distance_t goal) {
+ SetGoal(goal);
+ return Calculate(measurement);
+ }
/**
* Returns the next output of the PID controller.
@@ -240,21 +279,36 @@
* @param goal The new goal of the controller.
* @param constraints Velocity and acceleration constraints for goal.
*/
- double Calculate(units::meter_t measurement, units::meter_t goal,
- frc::TrapezoidProfile::Constraints constraints);
+ double Calculate(
+ Distance_t measurement, Distance_t goal,
+ typename frc::TrapezoidProfile<Distance>::Constraints constraints) {
+ SetConstraints(constraints);
+ return Calculate(measurement, goal);
+ }
/**
* Reset the previous error, the integral term, and disable the controller.
*/
- void Reset();
+ void Reset() { m_controller.Reset(); }
- void InitSendable(frc::SendableBuilder& builder) override;
+ void InitSendable(frc::SendableBuilder& builder) override {
+ builder.SetSmartDashboardType("ProfiledPIDController");
+ builder.AddDoubleProperty("p", [this] { return GetP(); },
+ [this](double value) { SetP(value); });
+ builder.AddDoubleProperty("i", [this] { return GetI(); },
+ [this](double value) { SetI(value); });
+ builder.AddDoubleProperty("d", [this] { return GetD(); },
+ [this](double value) { SetD(value); });
+ builder.AddDoubleProperty(
+ "goal", [this] { return GetGoal().position.template to<double>(); },
+ [this](double value) { SetGoal(Distance_t{value}); });
+ }
private:
frc2::PIDController m_controller;
- frc::TrapezoidProfile::State m_goal;
- frc::TrapezoidProfile::State m_setpoint;
- frc::TrapezoidProfile::Constraints m_constraints;
+ typename frc::TrapezoidProfile<Distance>::State m_goal;
+ typename frc::TrapezoidProfile<Distance>::State m_setpoint;
+ typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/RamseteController.h b/wpilibc/src/main/native/include/frc/controller/RamseteController.h
index 3a564aa..6ec6edc 100644
--- a/wpilibc/src/main/native/include/frc/controller/RamseteController.h
+++ b/wpilibc/src/main/native/include/frc/controller/RamseteController.h
@@ -55,6 +55,13 @@
RamseteController(double b, double 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.
+ */
+ RamseteController() : RamseteController(2.0, 0.7) {}
+
+ /**
* Returns true if the pose error is within tolerance of the reference.
*/
bool AtReference() const;
diff --git a/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
new file mode 100644
index 0000000..8f82cb9
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/MathExtras.h>
+
+namespace frc {
+/**
+ * A helper class that computes feedforward voltages for a simple
+ * permanent-magnet DC motor.
+ */
+template <class Distance>
+class SimpleMotorFeedforward {
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Acceleration =
+ units::compound_unit<Velocity, units::inverse<units::seconds>>;
+ using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
+ using ka_unit =
+ units::compound_unit<units::volts, units::inverse<Acceleration>>;
+
+ public:
+ constexpr SimpleMotorFeedforward() = default;
+
+ /**
+ * Creates a new SimpleMotorFeedforward with the specified gains.
+ *
+ * @param kS The static gain, in volts.
+ * @param kV The velocity gain, in volt seconds per distance.
+ * @param kA The acceleration gain, in volt seconds^2 per distance.
+ */
+ constexpr SimpleMotorFeedforward(
+ units::volt_t kS, units::unit_t<kv_unit> kV,
+ units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
+ : kS(kS), kV(kV), kA(kA) {}
+
+ /**
+ * Calculates the feedforward from the gains and setpoints.
+ *
+ * @param velocity The velocity setpoint, in distance per second.
+ * @param acceleration The acceleration setpoint, in distance per second^2.
+ * @return The computed feedforward, in volts.
+ */
+ constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
+ units::unit_t<Acceleration> acceleration =
+ units::unit_t<Acceleration>(0)) const {
+ return kS * wpi::sgn(velocity) + kV * velocity + kA * acceleration;
+ }
+
+ // 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 motor.
+ * @param acceleration The acceleration of the motor.
+ * @return The maximum possible velocity at the given acceleration.
+ */
+ constexpr units::unit_t<Velocity> MaxAchievableVelocity(
+ units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+ // Assume max velocity is positive
+ return (maxVoltage - kS - kA * acceleration) / 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.
+ */
+ constexpr units::unit_t<Velocity> MinAchievableVelocity(
+ units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+ // Assume min velocity is positive, ks flips sign
+ return (-maxVoltage + kS - kA * acceleration) / 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.
+ */
+ constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
+ units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+ return (maxVoltage - kS * wpi::sgn(velocity) - kV * velocity) / 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 motor.
+ * @param velocity The velocity of the motor.
+ * @return The minimum possible acceleration at the given velocity.
+ */
+ constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
+ units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+ return MaxAchievableAcceleration(-maxVoltage, velocity);
+ }
+
+ units::volt_t kS{0};
+ units::unit_t<kv_unit> kV{0};
+ units::unit_t<ka_unit> kA{0};
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
index 192d21b..c6b705a 100644
--- a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
+++ b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
@@ -30,12 +30,12 @@
* @code{.cpp}
* class Robot {
* public:
- * frc::Spark m_frontLeft{1};
- * frc::Spark m_rearLeft{2};
+ * frc::PWMVictorSPX m_frontLeft{1};
+ * frc::PWMVictorSPX m_rearLeft{2};
* frc::SpeedControllerGroup m_left{m_frontLeft, m_rearLeft};
*
- * frc::Spark m_frontRight{3};
- * frc::Spark m_rearRight{4};
+ * frc::PWMVictorSPX m_frontRight{3};
+ * frc::PWMVictorSPX m_rearRight{4};
* frc::SpeedControllerGroup m_right{m_frontRight, m_rearRight};
*
* frc::DifferentialDrive m_drive{m_left, m_right};
@@ -46,14 +46,14 @@
* @code{.cpp}
* class Robot {
* public:
- * frc::Spark m_frontLeft{1};
- * frc::Spark m_midLeft{2};
- * frc::Spark m_rearLeft{3};
+ * frc::PWMVictorSPX m_frontLeft{1};
+ * frc::PWMVictorSPX m_midLeft{2};
+ * frc::PWMVictorSPX m_rearLeft{3};
* frc::SpeedControllerGroup m_left{m_frontLeft, m_midLeft, m_rearLeft};
*
- * frc::Spark m_frontRight{4};
- * frc::Spark m_midRight{5};
- * frc::Spark m_rearRight{6};
+ * frc::PWMVictorSPX m_frontRight{4};
+ * frc::PWMVictorSPX m_midRight{5};
+ * frc::PWMVictorSPX m_rearRight{6};
* frc::SpeedControllerGroup m_right{m_frontRight, m_midRight, m_rearRight};
*
* frc::DifferentialDrive m_drive{m_left, m_right};
diff --git a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h
index 2161e82..46328a0 100644
--- a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h
+++ b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h
@@ -11,6 +11,10 @@
#include "Translation2d.h"
#include "Twist2d.h"
+namespace wpi {
+class json;
+} // namespace wpi
+
namespace frc {
/**
@@ -146,8 +150,8 @@
*
* @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 .5 degrees since the previous pose update,
- * the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
+ * 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.
*/
@@ -167,4 +171,9 @@
Translation2d m_translation;
Rotation2d m_rotation;
};
+
+void to_json(wpi::json& json, const Pose2d& pose);
+
+void from_json(const wpi::json& json, Pose2d& pose);
+
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h
index 9453e0c..b636513 100644
--- a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h
+++ b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h
@@ -8,7 +8,10 @@
#pragma once
#include <units/units.h>
-#include <wpi/math>
+
+namespace wpi {
+class json;
+} // namespace wpi
namespace frc {
@@ -175,4 +178,9 @@
double m_cos = 1;
double m_sin = 0;
};
+
+void to_json(wpi::json& json, const Rotation2d& rotation);
+
+void from_json(const wpi::json& json, Rotation2d& rotation);
+
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/geometry/Translation2d.h b/wpilibc/src/main/native/include/frc/geometry/Translation2d.h
index a53abf1..90c61e2 100644
--- a/wpilibc/src/main/native/include/frc/geometry/Translation2d.h
+++ b/wpilibc/src/main/native/include/frc/geometry/Translation2d.h
@@ -11,6 +11,10 @@
#include "Rotation2d.h"
+namespace wpi {
+class json;
+} // namespace wpi
+
namespace frc {
/**
@@ -211,4 +215,9 @@
units::meter_t m_x = 0_m;
units::meter_t m_y = 0_m;
};
+
+void to_json(wpi::json& json, const Translation2d& state);
+
+void from_json(const wpi::json& json, Translation2d& state);
+
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
index 34407b9..cb2121f 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
@@ -31,7 +31,8 @@
* empirical value may be larger than the physical measured value due to
* scrubbing effects.
*/
- explicit DifferentialDriveKinematics(units::meter_t trackWidth);
+ constexpr explicit DifferentialDriveKinematics(units::meter_t trackWidth)
+ : trackWidth(trackWidth) {}
/**
* Returns a chassis speed from left and right component velocities using
@@ -40,8 +41,11 @@
* @param wheelSpeeds The left and right velocities.
* @return The chassis speed.
*/
- ChassisSpeeds ToChassisSpeeds(
- const DifferentialDriveWheelSpeeds& wheelSpeeds) const;
+ constexpr ChassisSpeeds ToChassisSpeeds(
+ const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
+ return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
+ (wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad};
+ }
/**
* Returns left and right component velocities from a chassis speed using
@@ -51,10 +55,12 @@
* represent the chassis' speed.
* @return The left and right velocities.
*/
- DifferentialDriveWheelSpeeds ToWheelSpeeds(
- const ChassisSpeeds& chassisSpeeds) const;
+ constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
+ const ChassisSpeeds& chassisSpeeds) const {
+ return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad,
+ chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
+ }
- private:
- units::meter_t m_trackWidth;
+ units::meter_t trackWidth;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
index fd41d15..379839d 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
@@ -23,29 +23,38 @@
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*
- * Note: It is important to reset both your encoders to zero before you start
- * using this class. Only reset your encoders ONCE. You should not reset your
- * encoders even if you want to reset your robot's pose.
+ * 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.
*/
class DifferentialDriveOdometry {
public:
/**
* Constructs a DifferentialDriveOdometry object.
*
- * @param kinematics The differential drive kinematics for your drivetrain.
+ * @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
- explicit DifferentialDriveOdometry(DifferentialDriveKinematics kinematics,
+ explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d());
/**
* Resets the robot's position on the field.
*
+ * You NEED to reset your encoders (to zero) when calling this method.
+ *
+ * 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.
*/
- void ResetPosition(const Pose2d& pose) {
+ void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
m_pose = pose;
m_previousAngle = pose.Rotation();
+ m_gyroOffset = m_pose.Rotation() - gyroAngle;
+
+ m_prevLeftDistance = 0_m;
+ m_prevRightDistance = 0_m;
}
/**
@@ -55,46 +64,26 @@
const Pose2d& GetPose() const { return m_pose; }
/**
- * 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.
+ * 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 currentTime The current time.
- * @param angle The angle of the robot.
- * @param wheelSpeeds The current wheel speeds.
- *
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param leftDistance The distance traveled by the left encoder.
+ * @param rightDistance The distance traveled by the right encoder.
* @return The new pose of the robot.
*/
- const Pose2d& UpdateWithTime(units::second_t currentTime,
- const Rotation2d& angle,
- const DifferentialDriveWheelSpeeds& wheelSpeeds);
-
- /**
- * 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 angle The angle of the robot.
- * @param wheelSpeeds The current wheel speeds.
- *
- * @return The new pose of the robot.
- */
- const Pose2d& Update(const Rotation2d& angle,
- const DifferentialDriveWheelSpeeds& wheelSpeeds) {
- return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds);
- }
+ const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+ units::meter_t rightDistance);
private:
- DifferentialDriveKinematics m_kinematics;
Pose2d m_pose;
- units::second_t m_previousTime = -1_s;
+ Rotation2d m_gyroOffset;
Rotation2d m_previousAngle;
+
+ units::meter_t m_prevLeftDistance = 0_m;
+ units::meter_t m_prevRightDistance = 0_m;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
index e8816e1..697a6b0 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
@@ -31,19 +31,26 @@
* Constructs a MecanumDriveOdometry object.
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
+ * @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
+ const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d());
/**
* Resets the robot's position on the field.
*
+ * 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.
*/
- void ResetPosition(const Pose2d& pose) {
+ void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
m_pose = pose;
m_previousAngle = pose.Rotation();
+ m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
/**
@@ -61,13 +68,13 @@
* angular rate that is calculated from forward kinematics.
*
* @param currentTime The current time.
- * @param angle The angle of the robot.
+ * @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& UpdateWithTime(units::second_t currentTime,
- const Rotation2d& angle,
+ const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds);
/**
@@ -78,14 +85,15 @@
* This also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
- * @param angle The angle of the robot.
- * @param wheelSpeeds The current wheel speeds.
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
- const Pose2d& Update(const Rotation2d& angle,
+ const Pose2d& Update(const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
- return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds);
+ return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
+ wheelSpeeds);
}
private:
@@ -94,6 +102,7 @@
units::second_t m_previousTime = -1_s;
Rotation2d m_previousAngle;
+ Rotation2d m_gyroOffset;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
index fcd8087..1bdbcea 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
@@ -122,6 +122,22 @@
ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates);
/**
+ * 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 moduleStates The state of the modules as an std::array of type
+ * SwerveModuleState, NumModules long 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.
+ */
+ ChassisSpeeds ToChassisSpeeds(
+ std::array<SwerveModuleState, NumModules> moduleStates);
+
+ /**
* 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
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
index 138954d..b362aa0 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
@@ -63,6 +63,13 @@
"locations provided in constructor.");
std::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
+
+ return this->ToChassisSpeeds(moduleStates);
+}
+
+template <size_t NumModules>
+ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
+ std::array<SwerveModuleState, NumModules> moduleStates) {
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
for (size_t i = 0; i < NumModules; i++) {
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
index 1dbe178..8093ca5 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
@@ -36,19 +36,26 @@
* 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.
*/
SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
+ const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d());
/**
* Resets the robot's position on the field.
*
+ * 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.
*/
- void ResetPosition(const Pose2d& pose) {
+ void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
m_pose = pose;
m_previousAngle = pose.Rotation();
+ m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
/**
@@ -66,7 +73,7 @@
* angular rate that is calculated from forward kinematics.
*
* @param currentTime The current time.
- * @param angle The angle of the robot.
+ * @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.
@@ -75,7 +82,7 @@
*/
template <typename... ModuleStates>
const Pose2d& UpdateWithTime(units::second_t currentTime,
- const Rotation2d& angle,
+ const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates);
/**
@@ -86,7 +93,7 @@
* This also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
- * @param angle The angle of the robot.
+ * @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.
@@ -94,9 +101,9 @@
* @return The new pose of the robot.
*/
template <typename... ModuleStates>
- const Pose2d& Update(const Rotation2d& angle,
+ const Pose2d& Update(const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates) {
- return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle,
+ return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
moduleStates...);
}
@@ -106,6 +113,7 @@
units::second_t m_previousTime = -1_s;
Rotation2d m_previousAngle;
+ Rotation2d m_gyroOffset;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
index 1ff75ac..23852d0 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
@@ -10,20 +10,24 @@
namespace frc {
template <size_t NumModules>
SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
- SwerveDriveKinematics<NumModules> kinematics, const Pose2d& initialPose)
+ SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
+ const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
+ m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
template <size_t NumModules>
template <typename... ModuleStates>
const Pose2d& frc::SwerveDriveOdometry<NumModules>::UpdateWithTime(
- units::second_t currentTime, const Rotation2d& angle,
+ units::second_t currentTime, const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates) {
units::second_t deltaTime =
(m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
m_previousTime = currentTime;
+ auto angle = gyroAngle + m_gyroOffset;
+
auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...);
static_cast<void>(dtheta);
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h b/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h
index 97805ba..9fdc049 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h
@@ -11,6 +11,7 @@
#include <string>
#include <vector>
+#include <networktables/NetworkTableEntry.h>
#include <networktables/NetworkTableValue.h>
#include "frc/ErrorBase.h"
@@ -97,6 +98,16 @@
static void Delete(wpi::StringRef key);
/**
+ * Returns an NT Entry mapping to the specified key
+ *
+ * This is useful if an entry is used often, or is read and then modified.
+ *
+ * @param key the key
+ * @return the entry for the key
+ */
+ static nt::NetworkTableEntry GetEntry(wpi::StringRef key);
+
+ /**
* Maps the specified key to the specified value in this table.
*
* The value can be retrieved by calling the get method with a key that is
diff --git a/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h
index a8979c1..1e08a94 100644
--- a/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h
+++ b/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h
@@ -48,7 +48,8 @@
}
private:
- Eigen::Matrix<double, 6, 4> m_coefficients;
+ Eigen::Matrix<double, 6, 4> m_coefficients =
+ Eigen::Matrix<double, 6, 4>::Zero();
/**
* Returns the hermite basis matrix for cubic hermite spline interpolation.
diff --git a/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h
index 730eab4..632d3ad 100644
--- a/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h
+++ b/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h
@@ -48,7 +48,8 @@
}
private:
- Eigen::Matrix<double, 6, 6> m_coefficients;
+ Eigen::Matrix<double, 6, 6> m_coefficients =
+ Eigen::Matrix<double, 6, 6>::Zero();
/**
* Returns the hermite basis matrix for quintic hermite spline interpolation.
diff --git a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
index 0a384fb..96c2c6e 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
@@ -13,6 +13,10 @@
#include "frc/geometry/Pose2d.h"
+namespace wpi {
+class json;
+} // namespace wpi
+
namespace frc {
/**
@@ -48,6 +52,22 @@
curvature_t curvature{0.0};
/**
+ * Checks equality between this State and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const State& other) const;
+
+ /**
+ * Checks inequality between this State and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are not equal.
+ */
+ bool operator!=(const State& other) const;
+
+ /**
* Interpolates between two States.
*
* @param endValue The end value for the interpolation.
@@ -103,4 +123,9 @@
return startValue + (endValue - startValue) * t;
}
};
+
+void to_json(wpi::json& json, const Trajectory::State& state);
+
+void from_json(const wpi::json& json, Trajectory::State& state);
+
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h
index a739070..36118a0 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h
@@ -14,7 +14,11 @@
#include <units/units.h>
#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
+#include "frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
namespace frc {
@@ -89,6 +93,27 @@
}
/**
+ * 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.
+ */
+ void SetKinematics(MecanumDriveKinematics kinematics) {
+ AddConstraint(MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
+ }
+
+ /**
+ * 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.
+ */
+ template <size_t NumModules>
+ void SetKinematics(SwerveDriveKinematics<NumModules>& kinematics) {
+ AddConstraint(SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
+ }
+
+ /**
* Returns the starting velocity of the trajectory.
* @return The starting velocity of the trajectory.
*/
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h
new file mode 100644
index 0000000..700ed9c
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <wpi/StringRef.h>
+#include <wpi/Twine.h>
+
+#include "frc/trajectory/Trajectory.h"
+
+namespace frc {
+class TrajectoryUtil {
+ public:
+ TrajectoryUtil() = delete;
+
+ /**
+ * 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
+ *
+ * @return The interpolated state.
+ */
+ static void ToPathweaverJson(const Trajectory& trajectory,
+ const wpi::Twine& path);
+ /**
+ * 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.
+ */
+ static Trajectory FromPathweaverJson(const wpi::Twine& path);
+
+ /**
+ * Deserializes a Trajectory from PathWeaver-style JSON.
+
+ * @param json the string containing the serialized JSON
+
+ * @return the trajectory represented by the JSON
+ */
+ static std::string SerializeTrajectory(const Trajectory& trajectory);
+
+ /**
+ * Serializes a Trajectory to PathWeaver-style JSON.
+
+ * @param trajectory the trajectory to export
+
+ * @return the string containing the serialized JSON
+ */
+ static Trajectory DeserializeTrajectory(wpi::StringRef json_str);
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
index c528c9e..0479cfe 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -40,18 +40,27 @@
* `Calculate()` and to determine when the profile has completed via
* `IsFinished()`.
*/
+template <class Distance>
class TrapezoidProfile {
+ using Distance_t = units::unit_t<Distance>;
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Velocity_t = units::unit_t<Velocity>;
+ using Acceleration =
+ units::compound_unit<Velocity, units::inverse<units::seconds>>;
+ using Acceleration_t = units::unit_t<Acceleration>;
+
public:
class Constraints {
public:
- units::meters_per_second_t maxVelocity = 0_mps;
- units::meters_per_second_squared_t maxAcceleration = 0_mps_sq;
+ Velocity_t maxVelocity{0};
+ Acceleration_t maxAcceleration{0};
};
class State {
public:
- units::meter_t position = 0_m;
- units::meters_per_second_t velocity = 0_mps;
+ Distance_t position{0};
+ Velocity_t velocity{0};
bool operator==(const State& rhs) const {
return position == rhs.position && velocity == rhs.velocity;
}
@@ -66,7 +75,7 @@
* @param initial The initial state (usually the current state).
*/
TrapezoidProfile(Constraints constraints, State goal,
- State initial = State{0_m, 0_mps});
+ State initial = State{Distance_t(0), Velocity_t(0)});
TrapezoidProfile(const TrapezoidProfile&) = default;
TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
@@ -86,7 +95,7 @@
*
* @param target The target distance.
*/
- units::second_t TimeLeftUntil(units::meter_t target) const;
+ units::second_t TimeLeftUntil(Distance_t target) const;
/**
* Returns the total time the profile takes to reach the goal.
@@ -135,5 +144,6 @@
units::second_t m_endFullSpeed;
units::second_t m_endDeccel;
};
-
} // namespace frc
+
+#include "TrapezoidProfile.inc"
diff --git a/wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
similarity index 76%
rename from wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp
rename to wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
index 6026aa5..50f232d 100644
--- a/wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
@@ -5,12 +5,14 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include "frc/trajectory/TrapezoidProfile.h"
+#pragma once
-using namespace frc;
+#include <algorithm>
-TrapezoidProfile::TrapezoidProfile(Constraints constraints, State goal,
- State initial)
+namespace frc {
+template <class Distance>
+TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints,
+ State goal, State initial)
: m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
m_constraints(constraints),
m_initial(Direct(initial)),
@@ -24,30 +26,30 @@
// ended at zero velocity
units::second_t cutoffBegin =
m_initial.velocity / m_constraints.maxAcceleration;
- units::meter_t cutoffDistBegin =
+ Distance_t cutoffDistBegin =
cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
- units::meter_t cutoffDistEnd =
+ Distance_t 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
- units::meter_t fullTrapezoidDist =
+ Distance_t fullTrapezoidDist =
cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
units::second_t accelerationTime =
m_constraints.maxVelocity / m_constraints.maxAcceleration;
- units::meter_t fullSpeedDist =
+ Distance_t fullSpeedDist =
fullTrapezoidDist -
accelerationTime * accelerationTime * m_constraints.maxAcceleration;
// Handle the case where the profile never reaches full speed
- if (fullSpeedDist < 0_m) {
+ if (fullSpeedDist < Distance_t(0)) {
accelerationTime =
units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
- fullSpeedDist = 0_m;
+ fullSpeedDist = Distance_t(0);
}
m_endAccel = accelerationTime - cutoffBegin;
@@ -55,7 +57,9 @@
m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
}
-TrapezoidProfile::State TrapezoidProfile::Calculate(units::second_t t) const {
+template <class Distance>
+typename TrapezoidProfile<Distance>::State
+TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
State result = m_initial;
if (t < m_endAccel) {
@@ -83,9 +87,11 @@
return Direct(result);
}
-units::second_t TrapezoidProfile::TimeLeftUntil(units::meter_t target) const {
- units::meter_t position = m_initial.position * m_direction;
- units::meters_per_second_t velocity = m_initial.velocity * m_direction;
+template <class Distance>
+units::second_t TrapezoidProfile<Distance>::TimeLeftUntil(
+ Distance_t target) const {
+ Distance_t position = m_initial.position * m_direction;
+ Velocity_t velocity = m_initial.velocity * m_direction;
units::second_t endAccel = m_endAccel * m_direction;
units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
@@ -101,21 +107,19 @@
units::second_t endDeccel = m_endDeccel - endAccel - endFullSpeed;
endDeccel = units::math::max(endDeccel, 0_s);
- const units::meters_per_second_squared_t acceleration =
- m_constraints.maxAcceleration;
- const units::meters_per_second_squared_t decceleration =
- -m_constraints.maxAcceleration;
+ const Acceleration_t acceleration = m_constraints.maxAcceleration;
+ const Acceleration_t decceleration = -m_constraints.maxAcceleration;
- units::meter_t distToTarget = units::math::abs(target - position);
+ Distance_t distToTarget = units::math::abs(target - position);
- if (distToTarget < 1e-6_m) {
+ if (distToTarget < Distance_t(1e-6)) {
return 0_s;
}
- units::meter_t accelDist =
+ Distance_t accelDist =
velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
- units::meters_per_second_t deccelVelocity;
+ Velocity_t deccelVelocity;
if (endAccel > 0_s) {
deccelVelocity = units::math::sqrt(
units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
@@ -123,20 +127,20 @@
deccelVelocity = velocity;
}
- units::meter_t deccelDist =
+ Distance_t deccelDist =
deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
- deccelDist = units::math::max(deccelDist, 0_m);
+ deccelDist = units::math::max(deccelDist, Distance_t(0));
- units::meter_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
+ Distance_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
if (accelDist > distToTarget) {
accelDist = distToTarget;
- fullSpeedDist = 0_m;
- deccelDist = 0_m;
+ fullSpeedDist = Distance_t(0);
+ deccelDist = Distance_t(0);
} else if (accelDist + fullSpeedDist > distToTarget) {
fullSpeedDist = distToTarget - accelDist;
- deccelDist = 0_m;
+ deccelDist = Distance_t(0);
} else {
deccelDist = distToTarget - fullSpeedDist - accelDist;
}
@@ -156,3 +160,4 @@
return accelTime + fullSpeedTime + deccelTime;
}
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
new file mode 100644
index 0000000..50ace85
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/controller/SimpleMotorFeedforward.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+/**
+ * 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.
+ */
+class DifferentialDriveVoltageConstraint : public TrajectoryConstraint {
+ public:
+ /**
+ * 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.
+ */
+ DifferentialDriveVoltageConstraint(
+ SimpleMotorFeedforward<units::meter> feedforward,
+ DifferentialDriveKinematics kinematics, units::volt_t maxVoltage);
+
+ units::meters_per_second_t MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) override;
+
+ MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) override;
+
+ private:
+ SimpleMotorFeedforward<units::meter> m_feedforward;
+ DifferentialDriveKinematics m_kinematics;
+ units::volt_t m_maxVoltage;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
new file mode 100644
index 0000000..53e3953
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <cmath>
+
+#include <units/units.h>
+
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+/**
+ * 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.
+ */
+namespace frc {
+class MecanumDriveKinematicsConstraint : public TrajectoryConstraint {
+ public:
+ MecanumDriveKinematicsConstraint(MecanumDriveKinematics kinematics,
+ units::meters_per_second_t maxSpeed);
+
+ units::meters_per_second_t MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) override;
+
+ MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) override;
+
+ private:
+ MecanumDriveKinematics m_kinematics;
+ units::meters_per_second_t m_maxSpeed;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
new file mode 100644
index 0000000..a8ad870
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <cmath>
+
+#include <units/units.h>
+
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+/**
+ * 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.
+ */
+namespace frc {
+
+template <size_t NumModules>
+class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
+ public:
+ SwerveDriveKinematicsConstraint(
+ frc::SwerveDriveKinematics<NumModules> kinematics,
+ units::meters_per_second_t maxSpeed);
+
+ units::meters_per_second_t MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) override;
+
+ MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) override;
+
+ private:
+ frc::SwerveDriveKinematics<NumModules> m_kinematics;
+ units::meters_per_second_t m_maxSpeed;
+};
+} // namespace frc
+
+#include "SwerveDriveKinematicsConstraint.inc"
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
new file mode 100644
index 0000000..c3437d5
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+/**
+ * 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.
+ */
+
+namespace frc {
+
+template <size_t NumModules>
+SwerveDriveKinematicsConstraint<NumModules>::SwerveDriveKinematicsConstraint(
+ frc::SwerveDriveKinematics<NumModules> kinematics,
+ units::meters_per_second_t maxSpeed)
+ : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+
+template <size_t NumModules>
+units::meters_per_second_t
+SwerveDriveKinematicsConstraint<NumModules>::MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) {
+ auto xVelocity = velocity * pose.Rotation().Cos();
+ auto yVelocity = velocity * pose.Rotation().Sin();
+ auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
+ {xVelocity, yVelocity, velocity * curvature});
+ m_kinematics.NormalizeWheelSpeeds(&wheelSpeeds, m_maxSpeed);
+
+ auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
+}
+
+template <size_t NumModules>
+TrajectoryConstraint::MinMax
+SwerveDriveKinematicsConstraint<NumModules>::MinMaxAcceleration(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) {
+ return {};
+}
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h
deleted file mode 100644
index b1ff9e6..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h
+++ /dev/null
@@ -1,132 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-#include "frc/controller/ProfiledPIDController.h"
-#include "frc2/command/CommandBase.h"
-#include "frc2/command/CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that controls an output with a ProfiledPIDController. Runs forever
- * by default - to add exit conditions and/or other behavior, subclass this
- * class. The controller calculation and output are performed synchronously in
- * the command's execute() method.
- *
- * @see ProfiledPIDController
- */
-class ProfiledPIDCommand
- : public CommandHelper<CommandBase, ProfiledPIDCommand> {
- using State = frc::TrapezoidProfile::State;
-
- public:
- /**
- * Creates a new PIDCommand, which controls the given output with a
- * ProfiledPIDController.
- *
- * @param controller the controller that controls the output.
- * @param measurementSource the measurement of the process variable
- * @param goalSource the controller's goal
- * @param useOutput the controller's output
- * @param requirements the subsystems required by this command
- */
- ProfiledPIDCommand(frc::ProfiledPIDController controller,
- std::function<units::meter_t()> measurementSource,
- std::function<State()> goalSource,
- std::function<void(double, State)> useOutput,
- std::initializer_list<Subsystem*> requirements);
-
- /**
- * Creates a new PIDCommand, which controls the given output with a
- * ProfiledPIDController.
- *
- * @param controller the controller that controls the output.
- * @param measurementSource the measurement of the process variable
- * @param goalSource the controller's goal
- * @param useOutput the controller's output
- * @param requirements the subsystems required by this command
- */
- ProfiledPIDCommand(frc::ProfiledPIDController controller,
- std::function<units::meter_t()> measurementSource,
- std::function<units::meter_t()> goalSource,
- std::function<void(double, State)> useOutput,
- std::initializer_list<Subsystem*> requirements);
-
- /**
- * Creates a new PIDCommand, which controls the given output with a
- * ProfiledPIDController with a constant goal.
- *
- * @param controller the controller that controls the output.
- * @param measurementSource the measurement of the process variable
- * @param goal the controller's goal
- * @param useOutput the controller's output
- * @param requirements the subsystems required by this command
- */
- ProfiledPIDCommand(frc::ProfiledPIDController controller,
- std::function<units::meter_t()> measurementSource,
- State goal, std::function<void(double, State)> useOutput,
- std::initializer_list<Subsystem*> requirements);
-
- /**
- * Creates a new PIDCommand, which controls the given output with a
- * ProfiledPIDController with a constant goal.
- *
- * @param controller the controller that controls the output.
- * @param measurementSource the measurement of the process variable
- * @param goal the controller's goal
- * @param useOutput the controller's output
- * @param requirements the subsystems required by this command
- */
- ProfiledPIDCommand(frc::ProfiledPIDController controller,
- std::function<units::meter_t()> measurementSource,
- units::meter_t goal,
- std::function<void(double, State)> useOutput,
- std::initializer_list<Subsystem*> requirements);
-
- ProfiledPIDCommand(ProfiledPIDCommand&& other) = default;
-
- ProfiledPIDCommand(const ProfiledPIDCommand& other) = default;
-
- void Initialize() override;
-
- void Execute() override;
-
- void End(bool interrupted) override;
-
- /**
- * Gets the measurement of the process variable. Wraps the passed-in function
- * for readability.
- *
- * @return The measurement of the process variable
- */
- virtual units::meter_t GetMeasurement();
-
- /**
- * Gets the measurement of the process variable. Wraps the passed-in function
- * for readability.
- *
- * @return The measurement of the process variable
- */
- virtual void UseOutput(double output, State state);
-
- /**
- * Returns the ProfiledPIDController used by the command.
- *
- * @return The ProfiledPIDController
- */
- frc::ProfiledPIDController& GetController();
-
- protected:
- frc::ProfiledPIDController m_controller;
- std::function<units::meter_t()> m_measurement;
- std::function<State()> m_goal;
- std::function<void(double, State)> m_useOutput;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/wpilibc/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
deleted file mode 100644
index 4fa47b2..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-#include "frc/controller/ProfiledPIDController.h"
-#include "frc2/command/SubsystemBase.h"
-
-namespace frc2 {
-/**
- * A subsystem that uses a ProfiledPIDController to control an output. The
- * controller is run synchronously from the subsystem's periodic() method.
- *
- * @see ProfiledPIDController
- */
-class ProfiledPIDSubsystem : public SubsystemBase {
- using State = frc::TrapezoidProfile::State;
-
- public:
- /**
- * Creates a new ProfiledPIDSubsystem.
- *
- * @param controller the ProfiledPIDController to use
- */
- explicit ProfiledPIDSubsystem(frc::ProfiledPIDController controller);
-
- void Periodic() override;
-
- /**
- * Uses the output from the ProfiledPIDController.
- *
- * @param output the output of the ProfiledPIDController
- */
- virtual void UseOutput(double output, State state) = 0;
-
- /**
- * Returns the goal used by the ProfiledPIDController.
- *
- * @return the goal to be used by the controller
- */
- virtual State GetGoal() = 0;
-
- /**
- * Returns the measurement of the process variable used by the
- * ProfiledPIDController.
- *
- * @return the measurement of the process variable
- */
- virtual units::meter_t GetMeasurement() = 0;
-
- /**
- * Enables the PID control. Resets the controller.
- */
- virtual void Enable();
-
- /**
- * Disables the PID control. Sets output to zero.
- */
- virtual void Disable();
-
- /**
- * Returns the ProfiledPIDController.
- *
- * @return The controller.
- */
- frc::ProfiledPIDController& GetController();
-
- protected:
- frc::ProfiledPIDController m_controller;
- bool m_enabled;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibc/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
deleted file mode 100644
index 6864f53..0000000
--- a/wpilibc/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
+++ /dev/null
@@ -1,55 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#include <frc/trajectory/TrapezoidProfile.h>
-#include <frc2/Timer.h>
-
-#include <functional>
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-
-#pragma once
-
-namespace frc2 {
-/**
- * A command that runs a TrapezoidProfile. Useful for smoothly controlling
- * mechanism motion.
- *
- * @see TrapezoidProfile
- */
-class TrapezoidProfileCommand
- : public CommandHelper<CommandBase, TrapezoidProfileCommand> {
- public:
- /**
- * Creates a new TrapezoidProfileCommand that will execute the given
- * TrapezoidalProfile. Output will be piped to the provided consumer function.
- *
- * @param profile The motion profile to execute.
- * @param output The consumer for the profile output.
- */
- TrapezoidProfileCommand(
- frc::TrapezoidProfile profile,
- std::function<void(frc::TrapezoidProfile::State)> output,
- std::initializer_list<Subsystem*> requirements);
-
- void Initialize() override;
-
- void Execute() override;
-
- void End(bool interrupted) override;
-
- bool IsFinished() override;
-
- private:
- frc::TrapezoidProfile m_profile;
- std::function<void(frc::TrapezoidProfile::State)> m_output;
-
- Timer m_timer;
-};
-
-} // namespace frc2
diff --git a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp
index 888d183..0bb1002 100644
--- a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp
+++ b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp
@@ -45,20 +45,20 @@
class LinearFilterNoiseTest
: public testing::TestWithParam<LinearFilterNoiseTestType> {
protected:
- std::unique_ptr<frc::LinearFilter> m_filter;
+ std::unique_ptr<frc::LinearFilter<double>> m_filter;
void SetUp() override {
switch (GetParam()) {
case TEST_SINGLE_POLE_IIR: {
- m_filter = std::make_unique<frc::LinearFilter>(
- frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant,
- kFilterStep));
+ m_filter = std::make_unique<frc::LinearFilter<double>>(
+ frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
+ kFilterStep));
break;
}
case TEST_MOVAVG: {
- m_filter = std::make_unique<frc::LinearFilter>(
- frc::LinearFilter::MovingAverage(kMovAvgTaps));
+ m_filter = std::make_unique<frc::LinearFilter<double>>(
+ frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
break;
}
}
diff --git a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp
index 1f1476c..8db91b3 100644
--- a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp
+++ b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp
@@ -73,40 +73,41 @@
class LinearFilterOutputTest
: public testing::TestWithParam<LinearFilterOutputTestType> {
protected:
- std::unique_ptr<frc::LinearFilter> m_filter;
+ std::unique_ptr<frc::LinearFilter<double>> m_filter;
std::function<double(double)> m_data;
double m_expectedOutput = 0.0;
void SetUp() override {
switch (GetParam()) {
case TEST_SINGLE_POLE_IIR: {
- m_filter = std::make_unique<frc::LinearFilter>(
- frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant,
- kFilterStep));
+ m_filter = std::make_unique<frc::LinearFilter<double>>(
+ frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
+ kFilterStep));
m_data = GetData;
m_expectedOutput = kSinglePoleIIRExpectedOutput;
break;
}
case TEST_HIGH_PASS: {
- m_filter = std::make_unique<frc::LinearFilter>(
- frc::LinearFilter::HighPass(kHighPassTimeConstant, kFilterStep));
+ m_filter = std::make_unique<frc::LinearFilter<double>>(
+ frc::LinearFilter<double>::HighPass(kHighPassTimeConstant,
+ kFilterStep));
m_data = GetData;
m_expectedOutput = kHighPassExpectedOutput;
break;
}
case TEST_MOVAVG: {
- m_filter = std::make_unique<frc::LinearFilter>(
- frc::LinearFilter::MovingAverage(kMovAvgTaps));
+ m_filter = std::make_unique<frc::LinearFilter<double>>(
+ frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
m_data = GetData;
m_expectedOutput = kMovAvgExpectedOutput;
break;
}
case TEST_PULSE: {
- m_filter = std::make_unique<frc::LinearFilter>(
- frc::LinearFilter::MovingAverage(kMovAvgTaps));
+ m_filter = std::make_unique<frc::LinearFilter<double>>(
+ frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
m_data = GetPulseData;
m_expectedOutput = 0.0;
break;
diff --git a/wpilibc/src/test/native/cpp/MedianFilterTest.cpp b/wpilibc/src/test/native/cpp/MedianFilterTest.cpp
new file mode 100644
index 0000000..7fe7d2e
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/MedianFilterTest.cpp
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/MedianFilter.h"
+#include "gtest/gtest.h"
+
+TEST(MedianFilterTest, MedianFilterNotFullTestEven) {
+ frc::MedianFilter<double> filter{10};
+
+ filter.Calculate(3);
+ filter.Calculate(0);
+ filter.Calculate(4);
+
+ EXPECT_EQ(filter.Calculate(1000), 3.5);
+}
+
+TEST(MedianFilterTest, MedianFilterNotFullTestOdd) {
+ frc::MedianFilter<double> filter{10};
+
+ filter.Calculate(3);
+ filter.Calculate(0);
+ filter.Calculate(4);
+ filter.Calculate(7);
+
+ EXPECT_EQ(filter.Calculate(1000), 4);
+}
+
+TEST(MedianFilterTest, MedianFilterFullTestEven) {
+ frc::MedianFilter<double> filter{6};
+
+ filter.Calculate(3);
+ filter.Calculate(0);
+ filter.Calculate(0);
+ filter.Calculate(5);
+ filter.Calculate(4);
+ filter.Calculate(1000);
+
+ EXPECT_EQ(filter.Calculate(99), 4.5);
+}
+
+TEST(MedianFilterTest, MedianFilterFullTestOdd) {
+ frc::MedianFilter<double> filter{5};
+
+ filter.Calculate(3);
+ filter.Calculate(0);
+ filter.Calculate(5);
+ filter.Calculate(4);
+ filter.Calculate(1000);
+
+ EXPECT_EQ(filter.Calculate(99), 5);
+}
diff --git a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
index 7bdac2e..c68cb37 100644
--- a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
+++ b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
@@ -27,7 +27,7 @@
TEST_F(PIDInputOutputTest, ProportionalGainOutputTest) {
controller->SetP(4);
- EXPECT_DOUBLE_EQ(-.1, controller->Calculate(.025, 0));
+ EXPECT_DOUBLE_EQ(-0.1, controller->Calculate(0.025, 0));
}
TEST_F(PIDInputOutputTest, IntegralGainOutputTest) {
@@ -36,10 +36,10 @@
double out = 0;
for (int i = 0; i < 5; i++) {
- out = controller->Calculate(.025, 0);
+ out = controller->Calculate(0.025, 0);
}
- EXPECT_DOUBLE_EQ(-.5 * controller->GetPeriod().to<double>(), out);
+ EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().to<double>(), out);
}
TEST_F(PIDInputOutputTest, DerivativeGainOutputTest) {
@@ -47,6 +47,6 @@
controller->Calculate(0, 0);
- EXPECT_DOUBLE_EQ(-.01_s / controller->GetPeriod(),
- controller->Calculate(.0025, 0));
+ EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(),
+ controller->Calculate(0.0025, 0));
}
diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
index 2647252..8acaf92 100644
--- a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
+++ b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
@@ -15,30 +15,11 @@
using namespace frc;
-TEST(DifferentialDriveOdometry, OneIteration) {
- DifferentialDriveKinematics kinematics{0.381_m * 2};
- DifferentialDriveOdometry odometry{kinematics};
+TEST(DifferentialDriveOdometry, EncoderDistances) {
+ DifferentialDriveOdometry odometry{Rotation2d(45_deg)};
- odometry.ResetPosition(Pose2d());
- DifferentialDriveWheelSpeeds wheelSpeeds{0.02_mps, 0.02_mps};
- odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds());
- const auto& pose = odometry.UpdateWithTime(1_s, Rotation2d(), wheelSpeeds);
-
- EXPECT_NEAR(pose.Translation().X().to<double>(), 0.02, kEpsilon);
- EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, kEpsilon);
- EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
-}
-
-TEST(DifferentialDriveOdometry, QuarterCircle) {
- DifferentialDriveKinematics kinematics{0.381_m * 2};
- DifferentialDriveOdometry odometry{kinematics};
-
- odometry.ResetPosition(Pose2d());
- DifferentialDriveWheelSpeeds wheelSpeeds{
- 0.0_mps, units::meters_per_second_t(5 * wpi::math::pi)};
- odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds());
- const auto& pose =
- odometry.UpdateWithTime(1_s, Rotation2d(90_deg), wheelSpeeds);
+ const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
+ units::meter_t(5 * wpi::math::pi));
EXPECT_NEAR(pose.Translation().X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Translation().Y().to<double>(), 5.0, kEpsilon);
diff --git a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
index 0a6894d..5d990bf 100644
--- a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
+++ b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
@@ -18,11 +18,11 @@
Translation2d m_br{-12_m, -12_m};
MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
- MecanumDriveOdometry odometry{kinematics};
+ MecanumDriveOdometry odometry{kinematics, 0_rad};
};
TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
- odometry.ResetPosition(Pose2d());
+ odometry.ResetPosition(Pose2d(), 0_rad);
MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
3.536_mps};
@@ -35,7 +35,7 @@
}
TEST_F(MecanumDriveOdometryTest, TwoIterations) {
- odometry.ResetPosition(Pose2d());
+ odometry.ResetPosition(Pose2d(), 0_rad);
MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
@@ -47,7 +47,7 @@
}
TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
- odometry.ResetPosition(Pose2d());
+ odometry.ResetPosition(Pose2d(), 0_rad);
MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps,
39.986_mps};
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
@@ -57,3 +57,16 @@
EXPECT_NEAR(pose.Translation().Y().to<double>(), 12, 0.01);
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
}
+
+TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
+ odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
+
+ MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
+
+ odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
+ auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
+
+ EXPECT_NEAR(pose.Translation().X().to<double>(), 0.5, 0.01);
+ EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, 0.01);
+ EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
+}
diff --git a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
index ab81017..d0399dc 100644
--- a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
+++ b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
@@ -21,13 +21,13 @@
Translation2d m_br{-12_m, -12_m};
SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
- SwerveDriveOdometry<4> m_odometry{m_kinematics};
+ SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad};
};
TEST_F(SwerveDriveOdometryTest, TwoIterations) {
SwerveModuleState state{5_mps, Rotation2d()};
- m_odometry.ResetPosition(Pose2d());
+ m_odometry.ResetPosition(Pose2d(), 0_rad);
m_odometry.UpdateWithTime(0_s, Rotation2d(), SwerveModuleState(),
SwerveModuleState(), SwerveModuleState(),
SwerveModuleState());
@@ -47,7 +47,7 @@
SwerveModuleState zero{0_mps, Rotation2d()};
- m_odometry.ResetPosition(Pose2d());
+ m_odometry.ResetPosition(Pose2d(), 0_rad);
m_odometry.UpdateWithTime(0_s, Rotation2d(), zero, zero, zero, zero);
auto pose =
m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
@@ -56,3 +56,19 @@
EXPECT_NEAR(12.0, pose.Translation().Y().to<double>(), kEpsilon);
EXPECT_NEAR(90.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
}
+
+TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
+ m_odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
+
+ SwerveModuleState state{5_mps, Rotation2d()};
+
+ m_odometry.UpdateWithTime(0_s, Rotation2d(90_deg), SwerveModuleState(),
+ SwerveModuleState(), SwerveModuleState(),
+ SwerveModuleState());
+ auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state,
+ state, state);
+
+ EXPECT_NEAR(0.5, pose.Translation().X().to<double>(), kEpsilon);
+ EXPECT_NEAR(0.0, pose.Translation().Y().to<double>(), kEpsilon);
+ EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+}
diff --git a/wpilibc/src/test/native/cpp/main.cpp b/wpilibc/src/test/native/cpp/main.cpp
index 0e00efa..c6b6c58 100644
--- a/wpilibc/src/test/native/cpp/main.cpp
+++ b/wpilibc/src/test/native/cpp/main.cpp
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* 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. */
/*----------------------------------------------------------------------------*/
-#include <hal/HAL.h>
+#include <hal/HALBase.h>
#include "gtest/gtest.h"
diff --git a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
index 8254dd4..319be79 100644
--- a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
+++ b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
@@ -73,6 +73,22 @@
EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
a.Rotation().Radians().to<double>(), 1E-9);
+ // Check interior waypoints
+ bool interiorsGood = true;
+ for (auto& waypoint : waypoints) {
+ bool found = false;
+ for (auto& state : poses) {
+ if (std::abs(
+ waypoint.Distance(state.first.Translation()).to<double>()) <
+ 1E-9) {
+ found = true;
+ }
+ }
+ interiorsGood &= found;
+ }
+
+ EXPECT_TRUE(interiorsGood);
+
// Check last point.
EXPECT_NEAR(poses.back().first.Translation().X().to<double>(),
b.Translation().X().to<double>(), 1E-9);
@@ -97,3 +113,10 @@
Pose2d end{3_m, 0_m, Rotation2d{90_deg}};
Run(start, waypoints, end);
}
+
+TEST_F(CubicHermiteSplineTest, OneInterior) {
+ Pose2d start{0_m, 0_m, 0_rad};
+ std::vector<Translation2d> waypoints{Translation2d(2_m, 0_m)};
+ Pose2d end{4_m, 0_m, 0_rad};
+ Run(start, waypoints, end);
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
new file mode 100644
index 0000000..eb230da
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include <iostream>
+#include <memory>
+#include <vector>
+
+#include <units/units.h>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
+ // Pick an unreasonably large kA to ensure the constraint has to do some work
+ SimpleMotorFeedforward<units::meter> feedforward{1_V, 1_V / 1_mps,
+ 3_V / 1_mps_sq};
+ const DifferentialDriveKinematics kinematics{0.5_m};
+ const auto maxVoltage = 10_V;
+
+ auto config = TrajectoryConfig(12_fps, 12_fps_sq);
+ config.AddConstraint(
+ DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage));
+
+ auto trajectory = TestTrajectory::GetTrajectory(config);
+
+ units::second_t time = 0_s;
+ units::second_t dt = 20_ms;
+ units::second_t duration = trajectory.TotalTime();
+
+ while (time < duration) {
+ const Trajectory::State point = trajectory.Sample(time);
+ time += dt;
+
+ const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
+ point.velocity * point.curvature};
+
+ auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+ // 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
+ EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) <
+ maxVoltage + 0.05_V);
+ EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) >
+ -maxVoltage - 0.05_V);
+ EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) <
+ maxVoltage + 0.05_V);
+ EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) >
+ -maxVoltage - 0.05_V);
+ }
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
new file mode 100644
index 0000000..999d2bb
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrajectoryConfig.h"
+#include "frc/trajectory/TrajectoryUtil.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(TrajectoryJsonTest, DeserializeMatches) {
+ TrajectoryConfig config{12_fps, 12_fps_sq};
+ auto trajectory = TestTrajectory::GetTrajectory(config);
+
+ Trajectory deserialized;
+ EXPECT_NO_THROW(deserialized = TrajectoryUtil::DeserializeTrajectory(
+ TrajectoryUtil::SerializeTrajectory(trajectory)));
+ EXPECT_EQ(trajectory.States(), deserialized.States());
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
index ddd6a70..bd9ffef 100644
--- a/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
+++ b/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
@@ -25,12 +25,13 @@
}
TEST(TrapezoidProfileTest, ReachesGoal) {
- frc::TrapezoidProfile::Constraints constraints{1.75_mps, 0.75_mps_sq};
- frc::TrapezoidProfile::State goal{3_m, 0_mps};
- frc::TrapezoidProfile::State state;
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{3_m, 0_mps};
+ frc::TrapezoidProfile<units::meter>::State state;
for (int i = 0; i < 450; ++i) {
- frc::TrapezoidProfile profile{constraints, goal, state};
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_EQ(state, goal);
@@ -39,10 +40,11 @@
// Tests that decreasing the maximum velocity in the middle when it is already
// moving faster than the new max is handled correctly
TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
- frc::TrapezoidProfile::Constraints constraints{1.75_mps, 0.75_mps_sq};
- frc::TrapezoidProfile::State goal{12_m, 0_mps};
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};
- frc::TrapezoidProfile profile{constraints, goal};
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
auto lastPos = state.position;
@@ -51,7 +53,7 @@
constraints.maxVelocity = 0.75_mps;
}
- profile = frc::TrapezoidProfile{constraints, goal, state};
+ profile = frc::TrapezoidProfile<units::meter>{constraints, goal, state};
state = profile.Calculate(kDt);
auto estimatedVel = (state.position - lastPos) / kDt;
@@ -71,31 +73,33 @@
// There is some somewhat tricky code for dealing with going backwards
TEST(TrapezoidProfileTest, Backwards) {
- frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
- frc::TrapezoidProfile::State goal{-2_m, 0_mps};
- frc::TrapezoidProfile::State state;
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
+ frc::TrapezoidProfile<units::meter>::State state;
for (int i = 0; i < 400; ++i) {
- frc::TrapezoidProfile profile{constraints, goal, state};
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_EQ(state, goal);
}
TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
- frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
- frc::TrapezoidProfile::State goal{-2_m, 0_mps};
- frc::TrapezoidProfile::State state;
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
+ frc::TrapezoidProfile<units::meter>::State state;
for (int i = 0; i < 200; ++i) {
- frc::TrapezoidProfile profile{constraints, goal, state};
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_NE(state, goal);
goal = {0.0_m, 0.0_mps};
for (int i = 0; i < 550; ++i) {
- frc::TrapezoidProfile profile{constraints, goal, state};
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_EQ(state, goal);
@@ -103,30 +107,32 @@
// Checks to make sure that it hits top speed
TEST(TrapezoidProfileTest, TopSpeed) {
- frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
- frc::TrapezoidProfile::State goal{4_m, 0_mps};
- frc::TrapezoidProfile::State state;
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{4_m, 0_mps};
+ frc::TrapezoidProfile<units::meter>::State state;
for (int i = 0; i < 200; ++i) {
- frc::TrapezoidProfile profile{constraints, goal, state};
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
for (int i = 0; i < 2000; ++i) {
- frc::TrapezoidProfile profile{constraints, goal, state};
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_EQ(state, goal);
}
TEST(TrapezoidProfileTest, TimingToCurrent) {
- frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
- frc::TrapezoidProfile::State goal{2_m, 0_mps};
- frc::TrapezoidProfile::State state;
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
+ frc::TrapezoidProfile<units::meter>::State state;
for (int i = 0; i < 400; i++) {
- frc::TrapezoidProfile profile{constraints, goal, state};
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
}
@@ -135,16 +141,17 @@
TEST(TrapezoidProfileTest, TimingToGoal) {
using units::unit_cast;
- frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
- frc::TrapezoidProfile::State goal{2_m, 0_mps};
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
- frc::TrapezoidProfile profile{constraints, goal};
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
- profile = frc::TrapezoidProfile(constraints, goal, state);
+ profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
if (!reachedGoal && state == goal) {
// Expected value using for loop index is just an approximation since the
@@ -158,16 +165,17 @@
TEST(TrapezoidProfileTest, TimingBeforeGoal) {
using units::unit_cast;
- frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
- frc::TrapezoidProfile::State goal{2_m, 0_mps};
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
- frc::TrapezoidProfile profile{constraints, goal};
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
- profile = frc::TrapezoidProfile(constraints, goal, state);
+ profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
if (!reachedGoal &&
(units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
@@ -180,16 +188,17 @@
TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
using units::unit_cast;
- frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
- frc::TrapezoidProfile::State goal{-2_m, 0_mps};
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
- frc::TrapezoidProfile profile{constraints, goal};
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
- profile = frc::TrapezoidProfile(constraints, goal, state);
+ profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
if (!reachedGoal && state == goal) {
// Expected value using for loop index is just an approximation since the
@@ -203,16 +212,17 @@
TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
using units::unit_cast;
- frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
- frc::TrapezoidProfile::State goal{-2_m, 0_mps};
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
- frc::TrapezoidProfile profile{constraints, goal};
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
- profile = frc::TrapezoidProfile(constraints, goal, state);
+ profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
if (!reachedGoal &&
(units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
diff --git a/wpilibcExamples/.styleguide b/wpilibcExamples/.styleguide
index d9369fa..38c02cc 100644
--- a/wpilibcExamples/.styleguide
+++ b/wpilibcExamples/.styleguide
@@ -9,15 +9,16 @@
}
includeOtherLibs {
- ^HAL/
^cameraserver/
^cscore
^frc/
^frc2/
+ ^hal/
^networktables/
^ntcore
^opencv2/
^support/
+ ^units/
^vision/
^wpi/
}
diff --git a/wpilibcExamples/build.gradle b/wpilibcExamples/build.gradle
index 04a74bd..12279df 100644
--- a/wpilibcExamples/build.gradle
+++ b/wpilibcExamples/build.gradle
@@ -44,21 +44,6 @@
apply from: "${rootDir}/shared/opencv.gradle"
-ext {
- chipObjectComponents = ['commands']
- netCommComponents = ['commands']
- examplesMap.each { key, value ->
- chipObjectComponents << key.toString()
- netCommComponents << key.toString()
- }
- templatesMap.each { key, value ->
- chipObjectComponents << key.toString()
- netCommComponents << key.toString()
- }
-}
-
-apply from: "${rootDir}/shared/nilibraries.gradle"
-
model {
components {
commands(NativeLibrarySpec) {
@@ -67,6 +52,8 @@
binary.buildable = false
return
}
+ lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
+ lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
lib project: ':cscore', library: 'cscore', linkage: 'shared'
@@ -92,6 +79,8 @@
"${key}"(NativeExecutableSpec) {
targetBuildTypes 'debug'
binaries.all { binary ->
+ lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
+ lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
lib project: ':cscore', library: 'cscore', linkage: 'shared'
@@ -103,6 +92,9 @@
lib project: ':simulation:halsim_print', library: 'halsim_print', linkage: 'shared'
lib project: ':simulation:halsim_ds_nt', library: 'halsim_ds_nt', linkage: 'shared'
}
+ if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
}
sources {
cpp {
@@ -134,6 +126,8 @@
"${key}"(NativeExecutableSpec) {
targetBuildTypes 'debug'
binaries.all { binary ->
+ lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
+ lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
lib project: ':cscore', library: 'cscore', linkage: 'shared'
@@ -152,6 +146,9 @@
lib project: ':simulation:halsim_print', library: 'halsim_print', linkage: 'shared'
lib project: ':simulation:halsim_ds_nt', library: 'halsim_ds_nt', linkage: 'shared'
}
+ if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
}
sources {
cpp {
@@ -234,4 +231,7 @@
commandFile = new File("$projectDir/src/main/cpp/commands/commands.json")
}
+ext {
+ isCppCommands = true
+}
apply from: "${rootDir}/shared/examplecheck.gradle"
diff --git a/wpilibcExamples/publish.gradle b/wpilibcExamples/publish.gradle
index 927d88e..3bc1a09 100644
--- a/wpilibcExamples/publish.gradle
+++ b/wpilibcExamples/publish.gradle
@@ -12,8 +12,8 @@
def outputsFolder = file("$project.buildDir/outputs")
task cppExamplesZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = examplesZipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = examplesZipBaseName
from(licenseFile) {
into '/'
@@ -25,8 +25,8 @@
}
task cppTemplatesZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = templatesZipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = templatesZipBaseName
from(licenseFile) {
into '/'
@@ -38,8 +38,8 @@
}
task cppCommandsZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = commandsZipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = commandsZipBaseName
from(licenseFile) {
into '/'
diff --git a/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp
new file mode 100644
index 0000000..3cf84e7
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include <array>
+
+#include <frc/AddressableLED.h>
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+class Robot : public frc::TimedRobot {
+ static constexpr int kLength = 60;
+
+ // PWM port 0
+ // Must be a PWM header, not MXP or DIO
+ frc::AddressableLED m_led{9};
+ std::array<frc::AddressableLED::LEDData, kLength>
+ m_ledBuffer; // Reuse the buffer
+ // Store what the last hue of the first pixel is
+ int firstPixelHue = 0;
+
+ public:
+ void Rainbow() {
+ // For every pixel
+ for (int i = 0; i < kLength; i++) {
+ // Calculate the hue - hue is easier for rainbows because the color
+ // shape is a circle so only one value needs to precess
+ const auto pixelHue = (firstPixelHue + (i * 180 / kLength)) % 180;
+ // Set the value
+ m_ledBuffer[i].SetHSV(pixelHue, 255, 128);
+ }
+ // Increase by to make the rainbow "move"
+ firstPixelHue += 3;
+ // Check bounds
+ firstPixelHue %= 180;
+ }
+
+ void RobotInit() override {
+ // Default to a length of 60, start empty output
+ // Length is expensive to set, so only set it once, then just update data
+ m_led.SetLength(kLength);
+ m_led.SetData(m_ledBuffer);
+ m_led.Start();
+ }
+
+ void RobotPeriodic() override {
+ // Fill the buffer with a rainbow
+ Rainbow();
+ // Set the LEDs
+ m_led.SetData(m_ledBuffer);
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp
new file mode 100644
index 0000000..67f1e12
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/GenericHID.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/drive/DifferentialDrive.h>
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class.
+ * Runs the motors with split arcade steering and an Xbox controller.
+ */
+class Robot : public frc::TimedRobot {
+ frc::PWMVictorSPX m_leftMotor{0};
+ frc::PWMVictorSPX m_rightMotor{1};
+ frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
+ frc::XboxController m_driverController{0};
+
+ public:
+ void TeleopPeriodic() {
+ // Drive with split arcade style
+ // That means that the Y axis of the left stick moves forward
+ // and backward, and the X of the right stick turns left and right.
+ m_robotDrive.ArcadeDrive(
+ m_driverController.GetY(frc::GenericHID::JoystickHand::kLeftHand),
+ m_driverController.GetX(frc::GenericHID::JoystickHand::kRightHand));
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..aec6b1d
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc2/command/button/JoystickButton.h>
+#include <units/units.h>
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+
+ // Set up default drive command
+ m_drive.SetDefaultCommand(frc2::RunCommand(
+ [this] {
+ m_drive.ArcadeDrive(
+ m_driverController.GetY(frc::GenericHID::kLeftHand),
+ m_driverController.GetX(frc::GenericHID::kRightHand));
+ },
+ {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+
+ // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
+ frc2::JoystickButton(&m_driverController, 1)
+ .WhenPressed([this] { m_arm.SetGoal(2_rad); }, {&m_arm});
+
+ // Move the arm to neutral position when the 'B' button is pressed.
+ frc2::JoystickButton(&m_driverController, 1)
+ .WhenPressed([this] { m_arm.SetGoal(ArmConstants::kArmOffset); },
+ {&m_arm});
+
+ // While holding the shoulder button, drive at half speed
+ frc2::JoystickButton(&m_driverController, 6)
+ .WhenPressed([this] { m_drive.SetMaxOutput(.5); })
+ .WhenReleased([this] { m_drive.SetMaxOutput(1); });
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Runs the chosen command in autonomous
+ return new frc2::InstantCommand([] {});
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp
new file mode 100644
index 0000000..5531c5b
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/ArmSubsystem.h"
+
+#include "Constants.h"
+
+using namespace ArmConstants;
+using State = frc::TrapezoidProfile<units::radians>::State;
+
+ArmSubsystem::ArmSubsystem()
+ : frc2::ProfiledPIDSubsystem<units::radians>(
+ frc::ProfiledPIDController<units::radians>(
+ kP, 0, 0, {kMaxVelocity, kMaxAcceleration})),
+ m_motor(kMotorPort),
+ m_encoder(kEncoderPorts[0], kEncoderPorts[1]),
+ m_feedforward(kS, kCos, kV, kA) {
+ m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.to<double>());
+ // Start arm in neutral position
+ SetGoal(State{kArmOffset, 0_rad_per_s});
+}
+
+void ArmSubsystem::UseOutput(double output, State setpoint) {
+ // Calculate the feedforward from the sepoint
+ units::volt_t feedforward =
+ m_feedforward.Calculate(setpoint.position, setpoint.velocity);
+ // Add the feedforward to the PID output to get the motor output
+ m_motor.SetVoltage(units::volt_t(output) + feedforward);
+}
+
+units::radian_t ArmSubsystem::GetMeasurement() {
+ return units::radian_t(m_encoder.GetDistance()) + kArmOffset;
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..64be1b8
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+ : m_left1{kLeftMotor1Port},
+ m_left2{kLeftMotor2Port},
+ m_right1{kRightMotor1Port},
+ m_right2{kRightMotor2Port},
+ m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
+ m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+ // Set the distance per pulse for the encoders
+ m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void DriveSubsystem::Periodic() {
+ // Implementation of subsystem periodic method goes here.
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+ m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::ResetEncoders() {
+ m_leftEncoder.Reset();
+ m_rightEncoder.Reset();
+}
+
+double DriveSubsystem::GetAverageEncoderDistance() {
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+}
+
+frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+ m_drive.SetMaxOutput(maxOutput);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h
new file mode 100644
index 0000000..674633c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/math>
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants. This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
+
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
+
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+} // namespace DriveConstants
+
+namespace ArmConstants {
+constexpr int kMotorPort = 4;
+
+constexpr double kP = 1;
+
+// These are fake gains; in actuality these must be determined individually for
+// each robot
+constexpr auto kS = 1_V;
+constexpr auto kCos = 1_V;
+constexpr auto kV = 0.5_V * 1_s / 1_rad;
+constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad;
+
+constexpr auto kMaxVelocity = 3_rad_per_s;
+constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s);
+
+constexpr int kEncoderPorts[]{4, 5};
+constexpr int kEncoderPPR = 256;
+constexpr auto kEncoderDistancePerPulse = 2.0_rad * wpi::math::pi / kEncoderPPR;
+
+// The offset of the arm from the horizontal in its neutral position,
+// measured from the horizontal
+constexpr auto kArmOffset = 0.5_rad;
+} // namespace ArmConstants
+
+namespace AutoConstants {
+constexpr auto kAutoTimeoutSeconds = 12_s;
+constexpr auto kAutoShootTimeSeconds = 7_s;
+} // namespace AutoConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h
similarity index 60%
rename from wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
rename to wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h
index 2c70b8f..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -7,15 +7,17 @@
#pragma once
-#include <string>
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
-#include <frc/IterativeRobot.h>
-#include <frc/smartdashboard/SendableChooser.h>
+#include "RobotContainer.h"
-class Robot : public frc::IterativeRobot {
+class Robot : public frc::TimedRobot {
public:
void RobotInit() override;
void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
void AutonomousInit() override;
void AutonomousPeriodic() override;
void TeleopInit() override;
@@ -23,8 +25,9 @@
void TestPeriodic() override;
private:
- frc::SendableChooser<std::string> m_chooser;
- const std::string kAutoNameDefault = "Default";
- const std::string kAutoNameCustom = "My Auto";
- std::string m_autoSelected;
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h
new file mode 100644
index 0000000..fa07359
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/ConditionalCommand.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/WaitCommand.h>
+#include <frc2/command/WaitUntilCommand.h>
+
+#include "Constants.h"
+#include "subsystems/ArmSubsystem.h"
+#include "subsystems/DriveSubsystem.h"
+
+namespace ac = AutoConstants;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The driver's controller
+ frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+ // The robot's subsystems and commands are defined here...
+
+ // The robot's subsystems
+ DriveSubsystem m_drive;
+ ArmSubsystem m_arm;
+
+ // The chooser for the autonomous routines
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h
new file mode 100644
index 0000000..693f0b4
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/controller/ArmFeedforward.h>
+#include <frc2/command/ProfiledPIDSubsystem.h>
+#include <units/units.h>
+
+/**
+ * A robot arm subsystem that moves with a motion profile.
+ */
+class ArmSubsystem : public frc2::ProfiledPIDSubsystem<units::radians> {
+ using State = frc::TrapezoidProfile<units::radians>::State;
+
+ public:
+ ArmSubsystem();
+
+ void UseOutput(double output, State setpoint) override;
+
+ units::radian_t GetMeasurement() override;
+
+ private:
+ frc::PWMVictorSPX m_motor;
+ frc::Encoder m_encoder;
+ frc::ArmFeedforward m_feedforward;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..3ed1357
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+ DriveSubsystem();
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic() override;
+
+ // Subsystem methods go here.
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ void ArcadeDrive(double fwd, double rot);
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ void ResetEncoders();
+
+ /**
+ * Gets the average distance of the TWO encoders.
+ *
+ * @return the average of the TWO encoder readings
+ */
+ double GetAverageEncoderDistance();
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ frc::Encoder& GetLeftEncoder();
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ frc::Encoder& GetRightEncoder();
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive
+ * more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ void SetMaxOutput(double maxOutput);
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+
+ // The motor controllers
+ frc::PWMVictorSPX m_left1;
+ frc::PWMVictorSPX m_left2;
+ frc::PWMVictorSPX m_right1;
+ frc::PWMVictorSPX m_right2;
+
+ // The motors on the left side of the drive
+ frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+
+ // The motors on the right side of the drive
+ frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+
+ // The robot's drive
+ frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+
+ // The left-side drive encoder
+ frc::Encoder m_leftEncoder;
+
+ // The right-side drive encoder
+ frc::Encoder m_rightEncoder;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..aec6b1d
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc2/command/button/JoystickButton.h>
+#include <units/units.h>
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+
+ // Set up default drive command
+ m_drive.SetDefaultCommand(frc2::RunCommand(
+ [this] {
+ m_drive.ArcadeDrive(
+ m_driverController.GetY(frc::GenericHID::kLeftHand),
+ m_driverController.GetX(frc::GenericHID::kRightHand));
+ },
+ {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+
+ // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
+ frc2::JoystickButton(&m_driverController, 1)
+ .WhenPressed([this] { m_arm.SetGoal(2_rad); }, {&m_arm});
+
+ // Move the arm to neutral position when the 'B' button is pressed.
+ frc2::JoystickButton(&m_driverController, 1)
+ .WhenPressed([this] { m_arm.SetGoal(ArmConstants::kArmOffset); },
+ {&m_arm});
+
+ // While holding the shoulder button, drive at half speed
+ frc2::JoystickButton(&m_driverController, 6)
+ .WhenPressed([this] { m_drive.SetMaxOutput(.5); })
+ .WhenReleased([this] { m_drive.SetMaxOutput(1); });
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Runs the chosen command in autonomous
+ return new frc2::InstantCommand([] {});
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp
new file mode 100644
index 0000000..54a59ed
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/ArmSubsystem.h"
+
+#include "Constants.h"
+
+using namespace ArmConstants;
+using State = frc::TrapezoidProfile<units::radians>::State;
+
+ArmSubsystem::ArmSubsystem()
+ : frc2::TrapezoidProfileSubsystem<units::radians>(
+ {kMaxVelocity, kMaxAcceleration}, kArmOffset),
+ m_motor(kMotorPort),
+ m_feedforward(kS, kCos, kV, kA) {
+ m_motor.SetPID(kP, 0, 0);
+}
+
+void ArmSubsystem::UseState(State setpoint) {
+ // Calculate the feedforward from the sepoint
+ units::volt_t feedforward =
+ m_feedforward.Calculate(setpoint.position, setpoint.velocity);
+ // Add the feedforward to the PID output to get the motor output
+ m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+ setpoint.position.to<double>(), feedforward / 12_V);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..47c898e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+ : m_left1{kLeftMotor1Port},
+ m_left2{kLeftMotor2Port},
+ m_right1{kRightMotor1Port},
+ m_right2{kRightMotor2Port},
+ m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
+ m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+ // Set the distance per pulse for the encoders
+ m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void DriveSubsystem::Periodic() {
+ // Implementation of subsystem periodic method goes here.
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+ m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::ResetEncoders() {
+ m_leftEncoder.Reset();
+ m_rightEncoder.Reset();
+}
+
+double DriveSubsystem::GetAverageEncoderDistance() {
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
+}
+
+frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+ m_drive.SetMaxOutput(maxOutput);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h
new file mode 100644
index 0000000..674633c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/math>
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants. This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
+
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
+
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+} // namespace DriveConstants
+
+namespace ArmConstants {
+constexpr int kMotorPort = 4;
+
+constexpr double kP = 1;
+
+// These are fake gains; in actuality these must be determined individually for
+// each robot
+constexpr auto kS = 1_V;
+constexpr auto kCos = 1_V;
+constexpr auto kV = 0.5_V * 1_s / 1_rad;
+constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad;
+
+constexpr auto kMaxVelocity = 3_rad_per_s;
+constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s);
+
+constexpr int kEncoderPorts[]{4, 5};
+constexpr int kEncoderPPR = 256;
+constexpr auto kEncoderDistancePerPulse = 2.0_rad * wpi::math::pi / kEncoderPPR;
+
+// The offset of the arm from the horizontal in its neutral position,
+// measured from the horizontal
+constexpr auto kArmOffset = 0.5_rad;
+} // namespace ArmConstants
+
+namespace AutoConstants {
+constexpr auto kAutoTimeoutSeconds = 12_s;
+constexpr auto kAutoShootTimeSeconds = 7_s;
+} // namespace AutoConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h
new file mode 100644
index 0000000..7c8d261
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/SpeedController.h>
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor
+ * controller.
+ *
+ * <p>Has no actual functionality.
+ */
+class ExampleSmartMotorController : public frc::SpeedController {
+ public:
+ enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
+
+ /**
+ * Creates a new ExampleSmartMotorController.
+ *
+ * @param port The port for the controller.
+ */
+ explicit ExampleSmartMotorController(int port) {}
+
+ /**
+ * Example method for setting the PID gains of the smart controller.
+ *
+ * @param kp The proportional gain.
+ * @param ki The integral gain.
+ * @param kd The derivative gain.
+ */
+ void SetPID(double kp, double ki, double kd) {}
+
+ /**
+ * Example method for setting the setpoint of the smart controller in PID
+ * mode.
+ *
+ * @param mode The mode of the PID controller.
+ * @param setpoint The controller setpoint.
+ * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+ */
+ void SetSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
+
+ /**
+ * Places this motor controller in follower mode.
+ *
+ * @param master The master to follow.
+ */
+ void Follow(ExampleSmartMotorController master) {}
+
+ /**
+ * Returns the encoder distance.
+ *
+ * @return The current encoder distance.
+ */
+ double GetEncoderDistance() { return 0; }
+
+ /**
+ * Returns the encoder rate.
+ *
+ * @return The current encoder rate.
+ */
+ double GetEncoderRate() { return 0; }
+
+ /**
+ * Resets the encoder to zero distance.
+ */
+ void ResetEncoder() {}
+
+ void Set(double speed) override {}
+
+ double Get() const override { return 0; }
+
+ void SetInverted(bool isInverted) override {}
+
+ bool GetInverted() const override { return false; }
+
+ void Disable() override {}
+
+ void StopMotor() override {}
+
+ void PIDWrite(double output) override {}
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h
similarity index 60%
copy from wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
copy to wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h
index 2c70b8f..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -7,15 +7,17 @@
#pragma once
-#include <string>
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
-#include <frc/IterativeRobot.h>
-#include <frc/smartdashboard/SendableChooser.h>
+#include "RobotContainer.h"
-class Robot : public frc::IterativeRobot {
+class Robot : public frc::TimedRobot {
public:
void RobotInit() override;
void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
void AutonomousInit() override;
void AutonomousPeriodic() override;
void TeleopInit() override;
@@ -23,8 +25,9 @@
void TestPeriodic() override;
private:
- frc::SendableChooser<std::string> m_chooser;
- const std::string kAutoNameDefault = "Default";
- const std::string kAutoNameCustom = "My Auto";
- std::string m_autoSelected;
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h
new file mode 100644
index 0000000..fa07359
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/ConditionalCommand.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/WaitCommand.h>
+#include <frc2/command/WaitUntilCommand.h>
+
+#include "Constants.h"
+#include "subsystems/ArmSubsystem.h"
+#include "subsystems/DriveSubsystem.h"
+
+namespace ac = AutoConstants;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The driver's controller
+ frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+ // The robot's subsystems and commands are defined here...
+
+ // The robot's subsystems
+ DriveSubsystem m_drive;
+ ArmSubsystem m_arm;
+
+ // The chooser for the autonomous routines
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h
new file mode 100644
index 0000000..52f15f1
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/controller/ArmFeedforward.h>
+#include <frc2/command/TrapezoidProfileSubsystem.h>
+#include <units/units.h>
+
+#include "ExampleSmartMotorController.h"
+
+/**
+ * A robot arm subsystem that moves with a motion profile.
+ */
+class ArmSubsystem : public frc2::TrapezoidProfileSubsystem<units::radians> {
+ using State = frc::TrapezoidProfile<units::radians>::State;
+
+ public:
+ ArmSubsystem();
+
+ void UseState(State setpoint) override;
+
+ private:
+ ExampleSmartMotorController m_motor;
+ frc::ArmFeedforward m_feedforward;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..3ed1357
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+ DriveSubsystem();
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic() override;
+
+ // Subsystem methods go here.
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ void ArcadeDrive(double fwd, double rot);
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ void ResetEncoders();
+
+ /**
+ * Gets the average distance of the TWO encoders.
+ *
+ * @return the average of the TWO encoder readings
+ */
+ double GetAverageEncoderDistance();
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ frc::Encoder& GetLeftEncoder();
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ frc::Encoder& GetRightEncoder();
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive
+ * more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ void SetMaxOutput(double maxOutput);
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+
+ // The motor controllers
+ frc::PWMVictorSPX m_left1;
+ frc::PWMVictorSPX m_left2;
+ frc::PWMVictorSPX m_right1;
+ frc::PWMVictorSPX m_right2;
+
+ // The motors on the left side of the drive
+ frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+
+ // The motors on the right side of the drive
+ frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+
+ // The robot's drive
+ frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+
+ // The left-side drive encoder
+ frc::Encoder m_leftEncoder;
+
+ // The right-side drive encoder
+ frc::Encoder m_rightEncoder;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp
new file mode 100644
index 0000000..a4030a5
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp
@@ -0,0 +1,82 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/AnalogInput.h>
+#include <frc/DMA.h>
+#include <frc/DMASample.h>
+#include <frc/DigitalOutput.h>
+#include <frc/Encoder.h>
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+class Robot : public frc::TimedRobot {
+ frc::DMA m_dma; // DMA object
+
+ // DMA needs a trigger, can use an output as trigger.
+ // 8 Triggers exist per DMA object, can be triggered on any
+ // DigitalSource.
+ frc::DigitalOutput m_dmaTrigger{2};
+
+ // Analog input to read with DMA
+ frc::AnalogInput m_analogInput{0};
+
+ // Encoder to read with DMA
+ frc::Encoder m_encoder{0, 1};
+
+ public:
+ void RobotInit() override {
+ // Trigger on falling edge of dma trigger output
+ m_dma.SetExternalTrigger(&m_dmaTrigger, false, true);
+
+ // Add inputs we want to read via DMA
+ m_dma.AddAnalogInput(&m_analogInput);
+ m_dma.AddEncoder(&m_encoder);
+ m_dma.AddEncoderPeriod(&m_encoder);
+
+ // Make sure trigger is set to off.
+ m_dmaTrigger.Set(true);
+
+ // Start DMA. No triggers or inputs can be added after this call
+ // unless DMA is stopped.
+ m_dma.StartDMA(1024);
+ }
+
+ void RobotPeriodic() override {
+ // Manually Trigger DMA read
+ m_dmaTrigger.Set(false);
+
+ // Need to create a sample.
+ frc::DMASample sample;
+ int32_t remaining = 0;
+ int32_t status = 0;
+ // Update our sample. remaining is the number of samples remaining in the
+ // buffer status is more specfic error messages if readStatus is not OK.
+ // Wait 1ms if buffer is empty
+ HAL_DMAReadStatus readStatus =
+ sample.Update(&m_dma, 1_ms, &remaining, &status);
+
+ if (readStatus == HAL_DMA_OK) {
+ // Status value in all these reads should be checked, a non 0 value means
+ // value could not be read
+
+ // If DMA is good, values exist
+ auto encoderDistance = sample.GetEncoderDistance(&m_encoder, &status);
+ // Period is not scaled, and is a raw value
+ auto encoderPeriod = sample.GetEncoderPeriodRaw(&m_encoder, &status);
+ auto analogVoltage =
+ sample.GetAnalogInputVoltage(&m_analogInput, &status);
+
+ frc::SmartDashboard::PutNumber("Distance", encoderDistance);
+ frc::SmartDashboard::PutNumber("Period", encoderPeriod);
+ frc::SmartDashboard::PutNumber("Input", analogVoltage);
+ }
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
index 8dcc1ea..4d9c1ed 100644
--- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
@@ -7,11 +7,6 @@
#include "Drivetrain.h"
-frc::DifferentialDriveWheelSpeeds Drivetrain::GetSpeeds() const {
- return {units::meters_per_second_t(m_leftEncoder.GetRate()),
- units::meters_per_second_t(m_rightEncoder.GetRate())};
-}
-
void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
const auto leftOutput = m_leftPIDController.Calculate(
m_leftEncoder.GetRate(), speeds.left.to<double>());
@@ -28,5 +23,6 @@
}
void Drivetrain::UpdateOdometry() {
- m_odometry.Update(GetAngle(), GetSpeeds());
+ m_odometry.Update(GetAngle(), units::meter_t(m_leftEncoder.GetDistance()),
+ units::meter_t(m_rightEncoder.GetDistance()));
}
diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
index bf2ad98..3032bd9 100644
--- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
@@ -7,15 +7,14 @@
#pragma once
-#include <units/units.h>
-
#include <frc/AnalogGyro.h>
#include <frc/Encoder.h>
-#include <frc/Spark.h>
+#include <frc/PWMVictorSPX.h>
#include <frc/SpeedControllerGroup.h>
#include <frc/controller/PIDController.h>
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/kinematics/DifferentialDriveOdometry.h>
+#include <units/units.h>
#include <wpi/math>
/**
@@ -32,6 +31,9 @@
kEncoderResolution);
m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
kEncoderResolution);
+
+ m_leftEncoder.Reset();
+ m_rightEncoder.Reset();
}
/**
@@ -47,7 +49,6 @@
static constexpr units::radians_per_second_t kMaxAngularSpeed{
wpi::math::pi}; // 1/2 rotation per second
- frc::DifferentialDriveWheelSpeeds GetSpeeds() const;
void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
void Drive(units::meters_per_second_t xSpeed,
units::radians_per_second_t rot);
@@ -58,16 +59,16 @@
static constexpr double kWheelRadius = 0.0508; // meters
static constexpr int kEncoderResolution = 4096;
- frc::Spark m_leftMaster{1};
- frc::Spark m_leftFollower{2};
- frc::Spark m_rightMaster{3};
- frc::Spark m_rightFollower{4};
+ frc::PWMVictorSPX m_leftMaster{1};
+ frc::PWMVictorSPX m_leftFollower{2};
+ frc::PWMVictorSPX m_rightMaster{3};
+ frc::PWMVictorSPX m_rightFollower{4};
frc::SpeedControllerGroup m_leftGroup{m_leftMaster, m_leftFollower};
frc::SpeedControllerGroup m_rightGroup{m_rightMaster, m_rightFollower};
frc::Encoder m_leftEncoder{0, 1};
- frc::Encoder m_rightEncoder{0, 1};
+ frc::Encoder m_rightEncoder{2, 3};
frc2::PIDController m_leftPIDController{1.0, 0.0, 0.0};
frc2::PIDController m_rightPIDController{1.0, 0.0, 0.0};
@@ -75,5 +76,5 @@
frc::AnalogGyro m_gyro{0};
frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
- frc::DifferentialDriveOdometry m_odometry{m_kinematics};
+ frc::DifferentialDriveOdometry m_odometry{GetAngle()};
};
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..78bb48a
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc2/command/button/JoystickButton.h>
+
+#include "commands/DriveDistanceProfiled.h"
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+
+ // Set up default drive command
+ m_drive.SetDefaultCommand(frc2::RunCommand(
+ [this] {
+ m_drive.ArcadeDrive(
+ m_driverController.GetY(frc::GenericHID::kLeftHand),
+ m_driverController.GetX(frc::GenericHID::kRightHand));
+ },
+ {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+
+ // While holding the shoulder button, drive at half speed
+ frc2::JoystickButton(&m_driverController, 6)
+ .WhenPressed(&m_driveHalfSpeed)
+ .WhenReleased(&m_driveFullSpeed);
+
+ // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of
+ // 10 seconds
+ frc2::JoystickButton(&m_driverController, 1)
+ .WhenPressed(DriveDistanceProfiled(3_m, &m_drive).WithTimeout(10_s));
+
+ // Do the same thing as above when the 'B' button is pressed, but defined
+ // inline
+ frc2::JoystickButton(&m_driverController, 2)
+ .WhenPressed(
+ frc2::TrapezoidProfileCommand<units::meters>(
+ frc::TrapezoidProfile<units::meters>(
+ // Limit the max acceleration and velocity
+ {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration},
+ // End at desired position in meters; implicitly starts at 0
+ {3_m, 0_mps}),
+ // Pipe the profile state to the drive
+ [this](auto setpointState) {
+ m_drive.SetDriveStates(setpointState, setpointState);
+ },
+ // Require the drive
+ {&m_drive})
+ .BeforeStarting([this]() { m_drive.ResetEncoders(); })
+ .WithTimeout(10_s));
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Runs the chosen command in autonomous
+ return new frc2::InstantCommand([] {});
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp
new file mode 100644
index 0000000..7de607c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/DriveDistanceProfiled.h"
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+DriveDistanceProfiled::DriveDistanceProfiled(units::meter_t distance,
+ DriveSubsystem* drive)
+ : CommandHelper(
+ frc::TrapezoidProfile<units::meters>(
+ // Limit the max acceleration and velocity
+ {kMaxSpeed, kMaxAcceleration},
+ // End at desired position in meters; implicitly starts at 0
+ {distance, 0_mps}),
+ // Pipe the profile state to the drive
+ [drive](auto setpointState) {
+ drive->SetDriveStates(setpointState, setpointState);
+ },
+ // Require the drive
+ {drive}) {
+ // Reset drive encoders since we're starting at 0
+ drive->ResetEncoders();
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..cd8b3e4
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+ : m_leftMaster{kLeftMotor1Port},
+ m_leftSlave{kLeftMotor2Port},
+ m_rightMaster{kRightMotor1Port},
+ m_rightSlave{kRightMotor2Port},
+ m_feedforward{ks, kv, ka} {
+ m_leftSlave.Follow(m_leftMaster);
+ m_rightSlave.Follow(m_rightMaster);
+
+ m_leftMaster.SetPID(kp, 0, 0);
+ m_rightMaster.SetPID(kp, 0, 0);
+}
+
+void DriveSubsystem::Periodic() {
+ // Implementation of subsystem periodic method goes here.
+}
+
+void DriveSubsystem::SetDriveStates(
+ frc::TrapezoidProfile<units::meters>::State left,
+ frc::TrapezoidProfile<units::meters>::State right) {
+ m_leftMaster.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+ left.position.to<double>(),
+ m_feedforward.Calculate(left.velocity) / 12_V);
+ m_rightMaster.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+ right.position.to<double>(),
+ m_feedforward.Calculate(right.velocity) / 12_V);
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+ m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::ResetEncoders() {
+ m_leftMaster.ResetEncoder();
+ m_rightMaster.ResetEncoder();
+}
+
+units::meter_t DriveSubsystem::GetLeftEncoderDistance() {
+ return units::meter_t(m_leftMaster.GetEncoderDistance());
+}
+
+units::meter_t DriveSubsystem::GetRightEncoderDistance() {
+ return units::meter_t(m_rightMaster.GetEncoderDistance());
+}
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+ m_drive.SetMaxOutput(maxOutput);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h
new file mode 100644
index 0000000..bcf3356
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/math>
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants. This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
+
+// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+// These characterization values MUST be determined either experimentally or
+// theoretically for *your* robot's drive. The RobotPy Characterization
+// Toolsuite provides a convenient tool for obtaining these values for your
+// robot.
+constexpr auto ks = 1_V;
+constexpr auto kv = 0.8_V * 1_s / 1_m;
+constexpr auto ka = 0.15_V * 1_s * 1_s / 1_m;
+
+constexpr double kp = 1;
+
+constexpr auto kMaxSpeed = 3_mps;
+constexpr auto kMaxAcceleration = 3_mps_sq;
+
+} // namespace DriveConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h
new file mode 100644
index 0000000..7c8d261
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/SpeedController.h>
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor
+ * controller.
+ *
+ * <p>Has no actual functionality.
+ */
+class ExampleSmartMotorController : public frc::SpeedController {
+ public:
+ enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
+
+ /**
+ * Creates a new ExampleSmartMotorController.
+ *
+ * @param port The port for the controller.
+ */
+ explicit ExampleSmartMotorController(int port) {}
+
+ /**
+ * Example method for setting the PID gains of the smart controller.
+ *
+ * @param kp The proportional gain.
+ * @param ki The integral gain.
+ * @param kd The derivative gain.
+ */
+ void SetPID(double kp, double ki, double kd) {}
+
+ /**
+ * Example method for setting the setpoint of the smart controller in PID
+ * mode.
+ *
+ * @param mode The mode of the PID controller.
+ * @param setpoint The controller setpoint.
+ * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+ */
+ void SetSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
+
+ /**
+ * Places this motor controller in follower mode.
+ *
+ * @param master The master to follow.
+ */
+ void Follow(ExampleSmartMotorController master) {}
+
+ /**
+ * Returns the encoder distance.
+ *
+ * @return The current encoder distance.
+ */
+ double GetEncoderDistance() { return 0; }
+
+ /**
+ * Returns the encoder rate.
+ *
+ * @return The current encoder rate.
+ */
+ double GetEncoderRate() { return 0; }
+
+ /**
+ * Resets the encoder to zero distance.
+ */
+ void ResetEncoder() {}
+
+ void Set(double speed) override {}
+
+ double Get() const override { return 0; }
+
+ void SetInverted(bool isInverted) override {}
+
+ bool GetInverted() const override { return false; }
+
+ void Disable() override {}
+
+ void StopMotor() override {}
+
+ void PIDWrite(double output) override {}
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h
similarity index 60%
copy from wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
copy to wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h
index 2c70b8f..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -7,15 +7,17 @@
#pragma once
-#include <string>
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
-#include <frc/IterativeRobot.h>
-#include <frc/smartdashboard/SendableChooser.h>
+#include "RobotContainer.h"
-class Robot : public frc::IterativeRobot {
+class Robot : public frc::TimedRobot {
public:
void RobotInit() override;
void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
void AutonomousInit() override;
void AutonomousPeriodic() override;
void TeleopInit() override;
@@ -23,8 +25,9 @@
void TestPeriodic() override;
private:
- frc::SendableChooser<std::string> m_chooser;
- const std::string kAutoNameDefault = "Default";
- const std::string kAutoNameCustom = "My Auto";
- std::string m_autoSelected;
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h
new file mode 100644
index 0000000..af35be4
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/StartEndCommand.h>
+
+#include "Constants.h"
+#include "subsystems/DriveSubsystem.h"
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The driver's controller
+ frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+ // The robot's subsystems and commands are defined here...
+
+ // The robot's subsystems
+ DriveSubsystem m_drive;
+
+ frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
+ {}};
+ frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
+ {}};
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h
new file mode 100644
index 0000000..2199590
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/TrapezoidProfileCommand.h>
+
+#include "subsystems/DriveSubsystem.h"
+
+class DriveDistanceProfiled
+ : public frc2::CommandHelper<frc2::TrapezoidProfileCommand<units::meters>,
+ DriveDistanceProfiled> {
+ public:
+ DriveDistanceProfiled(units::meter_t distance, DriveSubsystem* drive);
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..f68cbbd
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <frc2/command/SubsystemBase.h>
+#include <units/units.h>
+
+#include "Constants.h"
+#include "ExampleSmartMotorController.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+ DriveSubsystem();
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic() override;
+
+ // Subsystem methods go here.
+
+ /**
+ * Attempts to follow the given drive states using offboard PID.
+ *
+ * @param left The left wheel state.
+ * @param right The right wheel state.
+ */
+ void SetDriveStates(frc::TrapezoidProfile<units::meters>::State left,
+ frc::TrapezoidProfile<units::meters>::State right);
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ void ArcadeDrive(double fwd, double rot);
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ void ResetEncoders();
+
+ /**
+ * Gets the distance of the left encoder.
+ *
+ * @return the average of the TWO encoder readings
+ */
+ units::meter_t GetLeftEncoderDistance();
+
+ /**
+ * Gets the distance of the right encoder.
+ *
+ * @return the average of the TWO encoder readings
+ */
+ units::meter_t GetRightEncoderDistance();
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive
+ * more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ void SetMaxOutput(double maxOutput);
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+
+ // The motor controllers
+ ExampleSmartMotorController m_leftMaster;
+ ExampleSmartMotorController m_leftSlave;
+ ExampleSmartMotorController m_rightMaster;
+ ExampleSmartMotorController m_rightSlave;
+
+ // A feedforward component for the drive
+ frc::SimpleMotorFeedforward<units::meters> m_feedforward;
+
+ // The robot's drive
+ frc::DifferentialDrive m_drive{m_leftMaster, m_rightMaster};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp
new file mode 100644
index 0000000..b75d99c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/DutyCycleEncoder.h>
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+class Robot : public frc::TimedRobot {
+ // Duty cycle encoder on channel 0
+ frc::DutyCycleEncoder m_dutyCycleEncoder{0};
+
+ public:
+ void RobotInit() override {
+ // Set to 0.5 units per rotation
+ m_dutyCycleEncoder.SetDistancePerRotation(0.5);
+ }
+
+ void RobotPeriodic() override {
+ // Connected can be checked, and uses the frequency of the encoder
+ auto connected = m_dutyCycleEncoder.IsConnected();
+
+ // Duty Cycle Frequency in Hz
+ auto frequency = m_dutyCycleEncoder.GetFrequency();
+
+ // Output of encoder
+ auto output = m_dutyCycleEncoder.Get();
+
+ // Output scaled by DistancePerPulse
+ auto distance = m_dutyCycleEncoder.GetDistance();
+
+ frc::SmartDashboard::PutBoolean("Connected", connected);
+ frc::SmartDashboard::PutNumber("Frequency", frequency);
+ frc::SmartDashboard::PutNumber("Output", output.to<double>());
+ frc::SmartDashboard::PutNumber("Distance", distance);
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/DutyCycleInput/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DutyCycleInput/cpp/Robot.cpp
new file mode 100644
index 0000000..1a113ae
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DutyCycleInput/cpp/Robot.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/DigitalInput.h>
+#include <frc/DutyCycle.h>
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+class Robot : public frc::TimedRobot {
+ frc::DigitalInput m_input{0}; // Input channel
+ frc::DutyCycle m_dutyCycle{m_input}; // Duty cycle input
+
+ public:
+ void RobotInit() override {}
+
+ void RobotPeriodic() override {
+ // Duty Cycle Frequency in Hz
+ auto frequency = m_dutyCycle.GetFrequency();
+
+ // Output of duty cycle, ranging from 0 to 1
+ // 1 is fully on, 0 is fully off
+ auto output = m_dutyCycle.GetOutput();
+
+ frc::SmartDashboard::PutNumber("Frequency", frequency);
+ frc::SmartDashboard::PutNumber("Duty Cycle", output);
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
index 95aac42..d423dd8 100644
--- a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
@@ -5,20 +5,22 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include <units/units.h>
-
#include <frc/Encoder.h>
#include <frc/Joystick.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/trajectory/TrapezoidProfile.h>
+#include <units/units.h>
+#include <wpi/math>
class Robot : public frc::TimedRobot {
public:
static constexpr units::second_t kDt = 20_ms;
- Robot() { m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5); }
+ Robot() {
+ m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * wpi::math::pi * 1.5);
+ }
void TeleopPeriodic() override {
if (m_joystick.GetRawButtonPressed(2)) {
@@ -39,8 +41,10 @@
// Create a PID controller whose setpoint's change is subject to maximum
// velocity and acceleration constraints.
- frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq};
- frc::ProfiledPIDController m_controller{1.3, 0.0, 0.7, m_constraints, kDt};
+ frc::TrapezoidProfile<units::meters>::Constraints m_constraints{1.75_mps,
+ 0.75_mps_sq};
+ frc::ProfiledPIDController<units::meters> m_controller{1.3, 0.0, 0.7,
+ m_constraints, kDt};
};
#ifndef RUNNING_FRC_TESTS
diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
index 9a15f31..5853054 100644
--- a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
@@ -5,18 +5,23 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include <frc/Encoder.h>
#include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
-#include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/trajectory/TrapezoidProfile.h>
+#include <units/units.h>
+#include <wpi/math>
+
+#include "ExampleSmartMotorController.h"
class Robot : public frc::TimedRobot {
public:
static constexpr units::second_t kDt = 20_ms;
- Robot() { m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5); }
+ Robot() {
+ // Note: These gains are fake, and will have to be tuned for your robot.
+ m_motor.SetPID(1.3, 0.0, 0.7);
+ }
void TeleopPeriodic() override {
if (m_joystick.GetRawButtonPressed(2)) {
@@ -28,27 +33,30 @@
// Create a motion profile with the given maximum velocity and maximum
// acceleration constraints for the next setpoint, the desired goal, and the
// current setpoint.
- frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
+ frc::TrapezoidProfile<units::meter> profile{m_constraints, m_goal,
+ m_setpoint};
// Retrieve the profiled setpoint for the next timestep. This setpoint moves
// toward the goal while obeying the constraints.
m_setpoint = profile.Calculate(kDt);
- // Run controller with profiled setpoint and update motor output
- double output = m_controller.Calculate(m_encoder.GetDistance(),
- m_setpoint.position.to<double>());
- m_motor.Set(output);
+ // Send setpoint to offboard controller PID
+ m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+ m_setpoint.position.to<double>(),
+ m_feedforward.Calculate(m_setpoint.velocity) / 12_V);
}
private:
frc::Joystick m_joystick{1};
- frc::Encoder m_encoder{1, 2};
- frc::PWMVictorSPX m_motor{1};
- frc2::PIDController m_controller{1.3, 0.0, 0.7, kDt};
+ ExampleSmartMotorController m_motor{1};
+ frc::SimpleMotorFeedforward<units::meters> m_feedforward{
+ // Note: These gains are fake, and will have to be tuned for your robot.
+ 1_V, 1.5_V * 1_s / 1_m};
- frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq};
- frc::TrapezoidProfile::State m_goal;
- frc::TrapezoidProfile::State m_setpoint;
+ frc::TrapezoidProfile<units::meters>::Constraints m_constraints{1.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meters>::State m_goal;
+ frc::TrapezoidProfile<units::meters>::State m_setpoint;
};
#ifndef RUNNING_FRC_TESTS
diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h
new file mode 100644
index 0000000..7c8d261
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/SpeedController.h>
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor
+ * controller.
+ *
+ * <p>Has no actual functionality.
+ */
+class ExampleSmartMotorController : public frc::SpeedController {
+ public:
+ enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
+
+ /**
+ * Creates a new ExampleSmartMotorController.
+ *
+ * @param port The port for the controller.
+ */
+ explicit ExampleSmartMotorController(int port) {}
+
+ /**
+ * Example method for setting the PID gains of the smart controller.
+ *
+ * @param kp The proportional gain.
+ * @param ki The integral gain.
+ * @param kd The derivative gain.
+ */
+ void SetPID(double kp, double ki, double kd) {}
+
+ /**
+ * Example method for setting the setpoint of the smart controller in PID
+ * mode.
+ *
+ * @param mode The mode of the PID controller.
+ * @param setpoint The controller setpoint.
+ * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+ */
+ void SetSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
+
+ /**
+ * Places this motor controller in follower mode.
+ *
+ * @param master The master to follow.
+ */
+ void Follow(ExampleSmartMotorController master) {}
+
+ /**
+ * Returns the encoder distance.
+ *
+ * @return The current encoder distance.
+ */
+ double GetEncoderDistance() { return 0; }
+
+ /**
+ * Returns the encoder rate.
+ *
+ * @return The current encoder rate.
+ */
+ double GetEncoderRate() { return 0; }
+
+ /**
+ * Resets the encoder to zero distance.
+ */
+ void ResetEncoder() {}
+
+ void Set(double speed) override {}
+
+ double Get() const override { return 0; }
+
+ void SetInverted(bool isInverted) override {}
+
+ bool GetInverted() const override { return false; }
+
+ void Disable() override {}
+
+ void StopMotor() override {}
+
+ void PIDWrite(double output) override {}
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp
index 1dbbfa5..3cfffd7 100644
--- a/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -8,6 +8,7 @@
#include <frc/Encoder.h>
#include <frc/TimedRobot.h>
#include <frc/smartdashboard/SmartDashboard.h>
+#include <wpi/math>
/**
* Sample program displaying the value of a quadrature encoder on the
@@ -42,7 +43,7 @@
* inch diameter (1.5inch radius) wheel, and that we want to measure
* distance in inches.
*/
- m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);
+ m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * wpi::math::pi * 1.5);
/* Defines the lowest rate at which the encoder will not be considered
* stopped, for the purposes of the GetStopped() method. Units are in
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
index 64be1b8..47c898e 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
@@ -35,7 +35,7 @@
}
double DriveSubsystem::GetAverageEncoderDistance() {
- return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
}
frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
index d3633b6..85be752 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
@@ -17,29 +17,22 @@
: PIDSubsystem(frc2::PIDController(kP, kI, kD)),
m_shooterMotor(kShooterMotorPort),
m_feederMotor(kFeederMotorPort),
- m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]) {
- m_controller.SetTolerance(kShooterToleranceRPS);
+ m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]),
+ m_shooterFeedforward(kS, kV) {
+ m_controller.SetTolerance(kShooterToleranceRPS.to<double>());
m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ SetSetpoint(kShooterTargetRPS.to<double>());
}
-void ShooterSubsystem::UseOutput(double output) {
- // Use a feedforward of the form kS + kV * velocity
- m_shooterMotor.Set(output + kSFractional + kVFractional * kShooterTargetRPS);
-}
-
-void ShooterSubsystem::Disable() {
- // Turn off motor when we disable, since useOutput(0) doesn't stop the motor
- // due to our feedforward
- frc2::PIDSubsystem::Disable();
- m_shooterMotor.Set(0);
+void ShooterSubsystem::UseOutput(double output, double setpoint) {
+ m_shooterMotor.SetVoltage(units::volt_t(output) +
+ m_shooterFeedforward.Calculate(kShooterTargetRPS));
}
bool ShooterSubsystem::AtSetpoint() { return m_controller.AtSetpoint(); }
double ShooterSubsystem::GetMeasurement() { return m_shooterEncoder.GetRate(); }
-double ShooterSubsystem::GetSetpoint() { return kShooterTargetRPS; }
-
void ShooterSubsystem::RunFeeder() { m_feederMotor.Set(kFeederSpeed); }
void ShooterSubsystem::StopFeeder() { m_feederMotor.Set(0); }
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
index 9fbcb27..1aef3ad 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
@@ -8,6 +8,7 @@
#pragma once
#include <units/units.h>
+#include <wpi/math>
/**
* The Constants header provides a convenient place for teams to hold robot-wide
@@ -19,50 +20,50 @@
*/
namespace DriveConstants {
-const int kLeftMotor1Port = 0;
-const int kLeftMotor2Port = 1;
-const int kRightMotor1Port = 2;
-const int kRightMotor2Port = 3;
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
-const int kLeftEncoderPorts[]{0, 1};
-const int kRightEncoderPorts[]{2, 3};
-const bool kLeftEncoderReversed = false;
-const bool kRightEncoderReversed = true;
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
-const int kEncoderCPR = 1024;
-const double kWheelDiameterInches = 6;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
- (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+ (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
} // namespace DriveConstants
namespace ShooterConstants {
-const int kEncoderPorts[]{4, 5};
-const bool kEncoderReversed = false;
-const int kEncoderCPR = 1024;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderPorts[]{4, 5};
+constexpr bool kEncoderReversed = false;
+constexpr int kEncoderCPR = 1024;
+constexpr double kEncoderDistancePerPulse =
// Distance units will be rotations
- 1. / static_cast<double>(kEncoderCPR);
+ 1.0 / static_cast<double>(kEncoderCPR);
-const int kShooterMotorPort = 4;
-const int kFeederMotorPort = 5;
+constexpr int kShooterMotorPort = 4;
+constexpr int kFeederMotorPort = 5;
-const double kShooterFreeRPS = 5300;
-const double kShooterTargetRPS = 4000;
-const double kShooterToleranceRPS = 50;
+constexpr auto kShooterFreeRPS = 5300_tr / 1_s;
+constexpr auto kShooterTargetRPS = 4000_tr / 1_s;
+constexpr auto kShooterToleranceRPS = 50_tr / 1_s;
-const double kP = 1;
-const double kI = 0;
-const double kD = 0;
+constexpr double kP = 1;
+constexpr double kI = 0;
+constexpr double kD = 0;
// On a real robot the feedforward constants should be empirically determined;
// these are reasonable guesses.
-const double kSFractional = .05;
-const double kVFractional =
- // Should have value 1 at free speed...
- 1. / kShooterFreeRPS;
+constexpr auto kS = 0.05_V;
+constexpr auto kV =
+ // Should have value 12V at free speed...
+ 12_V / kShooterFreeRPS;
-const double kFeederSpeed = .5;
+constexpr double kFeederSpeed = 0.5;
} // namespace ShooterConstants
namespace AutoConstants {
@@ -71,5 +72,5 @@
} // namespace AutoConstants
namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h
index 2ce1e1c..76dff49 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h
@@ -90,7 +90,7 @@
frc2::InstantCommand m_stopFeeder{[this] { m_shooter.StopFeeder(); },
{&m_shooter}};
- frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+ frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
{}};
frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
{}};
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
index 4d30d53..e8edd28 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
@@ -9,20 +9,18 @@
#include <frc/Encoder.h>
#include <frc/PWMVictorSPX.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc2/command/PIDSubsystem.h>
+#include <units/units.h>
class ShooterSubsystem : public frc2::PIDSubsystem {
public:
ShooterSubsystem();
- void UseOutput(double output) override;
-
- double GetSetpoint() override;
+ void UseOutput(double output, double setpoint) override;
double GetMeasurement() override;
- void Disable() override;
-
bool AtSetpoint();
void RunFeeder();
@@ -33,4 +31,5 @@
frc::PWMVictorSPX m_shooterMotor;
frc::PWMVictorSPX m_feederMotor;
frc::Encoder m_shooterEncoder;
+ frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
index a34c395..acdd07c 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
@@ -28,8 +28,6 @@
double Elevator::GetMeasurement() { return m_pot.Get(); }
-void Elevator::UseOutput(double d) { m_motor.Set(d); }
-
-double Elevator::GetSetpoint() { return m_setpoint; }
-
-void Elevator::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
+void Elevator::UseOutput(double output, double setpoint) {
+ m_motor.Set(output);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
index 7f2dedc..eeb94bb 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
@@ -26,10 +26,6 @@
// frc::SmartDashboard::PutData("Wrist Angle", &m_pot);
}
-double Wrist::GetSetpoint() { return m_setpoint; }
-
-void Wrist::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
-
double Wrist::GetMeasurement() { return m_pot.Get(); }
-void Wrist::UseOutput(double d) { m_motor.Set(d); }
+void Wrist::UseOutput(double output, double setpoint) { m_motor.Set(output); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
index a2c1e87..8acecbb 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
@@ -37,14 +37,7 @@
* by
* the subsystem.
*/
- void UseOutput(double d) override;
-
- double GetSetpoint() override;
-
- /**
- * Sets the setpoint for the subsystem.
- */
- void SetSetpoint(double setpoint);
+ void UseOutput(double output, double setpoint) override;
private:
frc::PWMVictorSPX m_motor{5};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
index e3ecc69..8b54e7e 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
@@ -35,14 +35,7 @@
* by
* the subsystem.
*/
- void UseOutput(double d) override;
-
- double GetSetpoint() override;
-
- /**
- * Sets the setpoint for the subsystem.
- */
- void SetSetpoint(double setpoint);
+ void UseOutput(double output, double setpoint) override;
private:
frc::PWMVictorSPX m_motor{6};
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
index aa1117a..6213333 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
@@ -8,6 +8,9 @@
#include "RobotContainer.h"
#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc2/command/PIDCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
#include <frc2/command/button/JoystickButton.h>
RobotContainer::RobotContainer() {
@@ -29,16 +32,39 @@
void RobotContainer::ConfigureButtonBindings() {
// Configure your button bindings here
+ // Assorted commands to be bound to buttons
+
// Stabilize robot to drive straight with gyro when left bumper is held
- frc2::JoystickButton(&m_driverController, 5).WhenHeld(&m_stabilizeDriving);
+ frc2::JoystickButton(&m_driverController, 5)
+ .WhenHeld(frc2::PIDCommand{
+ frc2::PIDController{dc::kStabilizationP, dc::kStabilizationI,
+ dc::kStabilizationD},
+ // Close the loop on the turn rate
+ [this] { return m_drive.GetTurnRate(); },
+ // Setpoint is 0
+ 0,
+ // Pipe the output to the turning controls
+ [this](double output) {
+ m_drive.ArcadeDrive(m_driverController.GetY(
+ frc::GenericHID::JoystickHand::kLeftHand),
+ output);
+ },
+ // Require the robot drive
+ {&m_drive}});
// Turn to 90 degrees when the 'X' button is pressed
- frc2::JoystickButton(&m_driverController, 3).WhenPressed(&m_turnTo90);
+ frc2::JoystickButton(&m_driverController, 3)
+ .WhenPressed(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s));
+
+ // Turn to -90 degrees with a profile when the 'A' button is pressed, with a 5
+ // second timeout
+ frc2::JoystickButton(&m_driverController, 1)
+ .WhenPressed(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s));
// While holding the shoulder button, drive at half speed
frc2::JoystickButton(&m_driverController, 6)
- .WhenPressed(&m_driveHalfSpeed)
- .WhenReleased(&m_driveFullSpeed);
+ .WhenPressed(frc2::InstantCommand{[this] { m_drive.SetMaxOutput(0.5); }})
+ .WhenReleased(frc2::InstantCommand{[this] { m_drive.SetMaxOutput(1); }});
}
frc2::Command* RobotContainer::GetAutonomousCommand() {
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
index cadb7f9..988535c 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
@@ -11,12 +11,12 @@
using namespace DriveConstants;
-TurnToAngle::TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive)
+TurnToAngle::TurnToAngle(units::degree_t target, DriveSubsystem* drive)
: CommandHelper(frc2::PIDController(kTurnP, kTurnI, kTurnD),
// Close loop on heading
- [drive] { return drive->GetHeading(); },
+ [drive] { return drive->GetHeading().to<double>(); },
// Set reference to target
- targetAngleDegrees,
+ target.to<double>(),
// Pipe output to turn robot
[drive](double output) { drive->ArcadeDrive(0, output); },
// Require the drive
@@ -26,9 +26,10 @@
// Set the controller tolerance - the delta tolerance ensures the robot is
// stationary at the setpoint before it is considered as having reached the
// reference
- m_controller.SetTolerance(kTurnToleranceDeg, kTurnRateToleranceDegPerS);
+ m_controller.SetTolerance(kTurnTolerance.to<double>(),
+ kTurnRateTolerance.to<double>());
AddRequirements({drive});
}
-bool TurnToAngle::IsFinished() { return m_controller.AtSetpoint(); }
+bool TurnToAngle::IsFinished() { return GetController().AtSetpoint(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp
new file mode 100644
index 0000000..c2baa90
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/TurnToAngleProfiled.h"
+
+#include <frc/controller/ProfiledPIDController.h>
+
+using namespace DriveConstants;
+
+TurnToAngleProfiled::TurnToAngleProfiled(units::degree_t target,
+ DriveSubsystem* drive)
+ : CommandHelper(
+ frc::ProfiledPIDController<units::radians>(
+ kTurnP, kTurnI, kTurnD, {kMaxTurnRate, kMaxTurnAcceleration}),
+ // Close loop on heading
+ [drive] { return drive->GetHeading(); },
+ // Set reference to target
+ target,
+ // Pipe output to turn robot
+ [drive](double output, auto setpointState) {
+ drive->ArcadeDrive(0, output);
+ },
+ // Require the drive
+ {drive}) {
+ // Set the controller to be continuous (because it is an angle controller)
+ GetController().EnableContinuousInput(-180_deg, 180_deg);
+ // Set the controller tolerance - the delta tolerance ensures the robot is
+ // stationary at the setpoint before it is considered as having reached the
+ // reference
+ GetController().SetTolerance(kTurnTolerance, kTurnRateTolerance);
+
+ AddRequirements({drive});
+}
+
+bool TurnToAngleProfiled::IsFinished() { return GetController().AtGoal(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
index 7e5ef96..c6d8d8c 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
@@ -35,7 +35,7 @@
}
double DriveSubsystem::GetAverageEncoderDistance() {
- return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
}
frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
@@ -46,10 +46,11 @@
m_drive.SetMaxOutput(maxOutput);
}
-double DriveSubsystem::GetHeading() {
- return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+units::degree_t DriveSubsystem::GetHeading() {
+ return units::degree_t(std::remainder(m_gyro.GetAngle(), 360) *
+ (kGyroReversed ? -1.0 : 1.0));
}
double DriveSubsystem::GetTurnRate() {
- return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.);
+ return m_gyro.GetRate() * (kGyroReversed ? -1.0 : 1.0);
}
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
index 0349c71..f423640 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
@@ -7,6 +7,9 @@
#pragma once
+#include <units/units.h>
+#include <wpi/math>
+
/**
* The Constants header provides a convenient place for teams to hold robot-wide
* numerical or bool constants. This should not be used for any other purpose.
@@ -17,42 +20,45 @@
*/
namespace DriveConstants {
-const int kLeftMotor1Port = 0;
-const int kLeftMotor2Port = 1;
-const int kRightMotor1Port = 2;
-const int kRightMotor2Port = 3;
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
-const int kLeftEncoderPorts[]{0, 1};
-const int kRightEncoderPorts[]{2, 3};
-const bool kLeftEncoderReversed = false;
-const bool kRightEncoderReversed = true;
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
-const int kEncoderCPR = 1024;
-const double kWheelDiameterInches = 6;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
- (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+ (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
-const bool kGyroReversed = true;
+constexpr bool kGyroReversed = true;
-const double kStabilizationP = 1;
-const double kStabilizationI = .5;
-const double kStabilizationD = 0;
+constexpr double kStabilizationP = 1;
+constexpr double kStabilizationI = 0.5;
+constexpr double kStabilizationD = 0;
-const double kTurnP = 1;
-const double kTurnI = 0;
-const double kTurnD = 0;
+constexpr double kTurnP = 1;
+constexpr double kTurnI = 0;
+constexpr double kTurnD = 0;
-const double kTurnToleranceDeg = 5;
-const double kTurnRateToleranceDegPerS = 10; // degrees per second
+constexpr auto kTurnTolerance = 5_deg;
+constexpr auto kTurnRateTolerance = 10_deg_per_s;
+
+constexpr auto kMaxTurnRate = 100_deg_per_s;
+constexpr auto kMaxTurnAcceleration = 300_deg_per_s / 1_s;
} // namespace DriveConstants
namespace AutoConstants {
-const double kAutoDriveDistanceInches = 60;
-const double kAutoBackupDistanceInches = 20;
-const double kAutoDriveSpeed = .5;
+constexpr double kAutoDriveDistanceInches = 60;
+constexpr double kAutoBackupDistanceInches = 20;
+constexpr double kAutoDriveSpeed = 0.5;
} // namespace AutoConstants
namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
index f480536..37c7e4f 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
@@ -12,9 +12,6 @@
#include <frc/smartdashboard/SendableChooser.h>
#include <frc2/command/Command.h>
#include <frc2/command/InstantCommand.h>
-#include <frc2/command/PIDCommand.h>
-#include <frc2/command/ParallelRaceGroup.h>
-#include <frc2/command/RunCommand.h>
#include "Constants.h"
#include "commands/TurnToAngle.h"
@@ -45,34 +42,6 @@
// The robot's subsystems
DriveSubsystem m_drive;
- // Assorted commands to be bound to buttons
-
- // Turn to 90 degrees, with a 5 second timeout
- frc2::ParallelRaceGroup m_turnTo90 =
- TurnToAngle{90, &m_drive}.WithTimeout(5_s);
-
- // Stabilize the robot while driving
- frc2::PIDCommand m_stabilizeDriving{
- frc2::PIDController{dc::kStabilizationP, dc::kStabilizationI,
- dc::kStabilizationD},
- // Close the loop on the turn rate
- [this] { return m_drive.GetTurnRate(); },
- // Setpoint is 0
- 0,
- // Pipe the output to the turning controls
- [this](double output) {
- m_drive.ArcadeDrive(
- m_driverController.GetY(frc::GenericHID::JoystickHand::kLeftHand),
- output);
- },
- // Require the robot drive
- {&m_drive}};
-
- frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
- {}};
- frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
- {}};
-
// The chooser for the autonomous routines
frc::SendableChooser<frc2::Command*> m_chooser;
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h
index c7e875e..4afa20e 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h
@@ -23,7 +23,7 @@
* @param targetAngleDegrees The angle to turn to
* @param drive The drive subsystem to use
*/
- TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive);
+ TurnToAngle(units::degree_t target, DriveSubsystem* drive);
bool IsFinished() override;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h
new file mode 100644
index 0000000..0b52044
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/ProfiledPIDCommand.h>
+
+#include "subsystems/DriveSubsystem.h"
+
+/**
+ * A command that will turn the robot to the specified angle using a motion
+ * profile.
+ */
+class TurnToAngleProfiled
+ : public frc2::CommandHelper<frc2::ProfiledPIDCommand<units::radians>,
+ TurnToAngleProfiled> {
+ public:
+ /**
+ * Turns to robot to the specified angle using a motion profile.
+ *
+ * @param targetAngleDegrees The angle to turn to
+ * @param drive The drive subsystem to use
+ */
+ TurnToAngleProfiled(units::degree_t targetAngleDegrees,
+ DriveSubsystem* drive);
+
+ bool IsFinished() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
index cada816..c4cd21c 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
@@ -13,6 +13,7 @@
#include <frc/SpeedControllerGroup.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc2/command/SubsystemBase.h>
+#include <units/units.h>
#include "Constants.h"
@@ -74,7 +75,7 @@
*
* @return the robot's heading in degrees, from 180 to 180
*/
- double GetHeading();
+ units::degree_t GetHeading();
/**
* Returns the turn rate of the robot.
diff --git a/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c b/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c
index dedb0f2..ae5833e 100644
--- a/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c
+++ b/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -18,7 +18,7 @@
#include <stdio.h>
-#include "hal/HAL.h"
+#include <hal/HAL.h>
enum DriverStationMode {
DisabledMode,
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
index 64be1b8..47c898e 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
@@ -35,7 +35,7 @@
}
double DriveSubsystem::GetAverageEncoderDistance() {
- return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
}
frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
index b09572e..d465f6c 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
@@ -7,6 +7,8 @@
#pragma once
+#include <wpi/math>
+
/**
* The Constants header provides a convenient place for teams to hold robot-wide
* numerical or bool constants. This should not be used for any other purpose.
@@ -17,34 +19,34 @@
*/
namespace DriveConstants {
-const int kLeftMotor1Port = 0;
-const int kLeftMotor2Port = 1;
-const int kRightMotor1Port = 2;
-const int kRightMotor2Port = 3;
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
-const int kLeftEncoderPorts[]{0, 1};
-const int kRightEncoderPorts[]{2, 3};
-const bool kLeftEncoderReversed = false;
-const bool kRightEncoderReversed = true;
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
-const int kEncoderCPR = 1024;
-const double kWheelDiameterInches = 6;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
- (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+ (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
} // namespace DriveConstants
namespace HatchConstants {
-const int kHatchSolenoidModule = 0;
-const int kHatchSolenoidPorts[]{0, 1};
+constexpr int kHatchSolenoidModule = 0;
+constexpr int kHatchSolenoidPorts[]{0, 1};
} // namespace HatchConstants
namespace AutoConstants {
-const double kAutoDriveDistanceInches = 60;
-const double kAutoBackupDistanceInches = 20;
-const double kAutoDriveSpeed = .5;
+constexpr double kAutoDriveDistanceInches = 60;
+constexpr double kAutoBackupDistanceInches = 20;
+constexpr double kAutoDriveSpeed = 0.5;
} // namespace AutoConstants
namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h
index b7c57a3..106812b 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h
@@ -63,7 +63,7 @@
frc2::InstantCommand m_grabHatch{[this] { m_hatch.GrabHatch(); }, {&m_hatch}};
frc2::InstantCommand m_releaseHatch{[this] { m_hatch.ReleaseHatch(); },
{&m_hatch}};
- frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+ frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
{}};
frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
{}};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp
index 839bb87..b35b2c6 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp
@@ -10,6 +10,6 @@
HalveDriveSpeed::HalveDriveSpeed(DriveSubsystem* subsystem)
: m_drive(subsystem) {}
-void HalveDriveSpeed::Initialize() { m_drive->SetMaxOutput(.5); }
+void HalveDriveSpeed::Initialize() { m_drive->SetMaxOutput(0.5); }
void HalveDriveSpeed::End(bool interrupted) { m_drive->SetMaxOutput(1); }
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
index 64be1b8..47c898e 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
@@ -35,7 +35,7 @@
}
double DriveSubsystem::GetAverageEncoderDistance() {
- return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
}
frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
index b09572e..d465f6c 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
@@ -7,6 +7,8 @@
#pragma once
+#include <wpi/math>
+
/**
* The Constants header provides a convenient place for teams to hold robot-wide
* numerical or bool constants. This should not be used for any other purpose.
@@ -17,34 +19,34 @@
*/
namespace DriveConstants {
-const int kLeftMotor1Port = 0;
-const int kLeftMotor2Port = 1;
-const int kRightMotor1Port = 2;
-const int kRightMotor2Port = 3;
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
-const int kLeftEncoderPorts[]{0, 1};
-const int kRightEncoderPorts[]{2, 3};
-const bool kLeftEncoderReversed = false;
-const bool kRightEncoderReversed = true;
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
-const int kEncoderCPR = 1024;
-const double kWheelDiameterInches = 6;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
- (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+ (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
} // namespace DriveConstants
namespace HatchConstants {
-const int kHatchSolenoidModule = 0;
-const int kHatchSolenoidPorts[]{0, 1};
+constexpr int kHatchSolenoidModule = 0;
+constexpr int kHatchSolenoidPorts[]{0, 1};
} // namespace HatchConstants
namespace AutoConstants {
-const double kAutoDriveDistanceInches = 60;
-const double kAutoBackupDistanceInches = 20;
-const double kAutoDriveSpeed = .5;
+constexpr double kAutoDriveDistanceInches = 60;
+constexpr double kAutoBackupDistanceInches = 20;
+constexpr double kAutoDriveSpeed = 0.5;
} // namespace AutoConstants
namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
index 3d44730..6550085 100644
--- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
@@ -9,7 +9,7 @@
#include <frc/AnalogGyro.h>
#include <frc/Encoder.h>
-#include <frc/Spark.h>
+#include <frc/PWMVictorSPX.h>
#include <frc/controller/PIDController.h>
#include <frc/geometry/Translation2d.h>
#include <frc/kinematics/MecanumDriveKinematics.h>
@@ -45,15 +45,15 @@
wpi::math::pi}; // 1/2 rotation per second
private:
- frc::Spark m_frontLeftMotor{1};
- frc::Spark m_frontRightMotor{2};
- frc::Spark m_backLeftMotor{3};
- frc::Spark m_backRightMotor{4};
+ frc::PWMVictorSPX m_frontLeftMotor{1};
+ frc::PWMVictorSPX m_frontRightMotor{2};
+ frc::PWMVictorSPX m_backLeftMotor{3};
+ frc::PWMVictorSPX m_backRightMotor{4};
frc::Encoder m_frontLeftEncoder{0, 1};
- frc::Encoder m_frontRightEncoder{0, 1};
- frc::Encoder m_backLeftEncoder{0, 1};
- frc::Encoder m_backRightEncoder{0, 1};
+ frc::Encoder m_frontRightEncoder{2, 3};
+ frc::Encoder m_backLeftEncoder{4, 5};
+ frc::Encoder m_backRightEncoder{6, 7};
frc::Translation2d m_frontLeftLocation{0.381_m, 0.381_m};
frc::Translation2d m_frontRightLocation{0.381_m, -0.381_m};
@@ -71,5 +71,5 @@
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
m_backRightLocation};
- frc::MecanumDriveOdometry m_odometry{m_kinematics};
+ frc::MecanumDriveOdometry m_odometry{m_kinematics, GetAngle()};
};
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..c7379cb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/controller/PIDController.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc/trajectory/Trajectory.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/MecanumControllerCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/button/JoystickButton.h>
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+const frc::MecanumDriveKinematics DriveConstants::kDriveKinematics{
+ frc::Translation2d(kWheelBase / 2, kTrackWidth / 2),
+ frc::Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+ frc::Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+ frc::Translation2d(-kWheelBase / 2, -kTrackWidth / 2)};
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+
+ // Set up default drive command
+ m_drive.SetDefaultCommand(frc2::RunCommand(
+ [this] {
+ m_drive.Drive(m_driverController.GetY(frc::GenericHID::kLeftHand),
+ m_driverController.GetX(frc::GenericHID::kRightHand),
+ m_driverController.GetX(frc::GenericHID::kLeftHand),
+ false);
+ },
+ {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+
+ // While holding the shoulder button, drive at half speed
+ frc2::JoystickButton(&m_driverController, 6)
+ .WhenPressed(&m_driveHalfSpeed)
+ .WhenReleased(&m_driveFullSpeed);
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Set up config for trajectory
+ frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
+ AutoConstants::kMaxAcceleration);
+ // Add kinematics to ensure max speed is actually obeyed
+ config.SetKinematics(DriveConstants::kDriveKinematics);
+
+ // An example trajectory to follow. All units in meters.
+ auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ // Start at the origin facing the +X direction
+ frc::Pose2d(0_m, 0_m, frc::Rotation2d(0_deg)),
+ // Pass through these two interior waypoints, making an 's' curve path
+ {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
+ // End 3 meters straight ahead of where we started, facing forward
+ frc::Pose2d(3_m, 0_m, frc::Rotation2d(0_deg)),
+ // Pass the config
+ config);
+
+ frc2::MecanumControllerCommand mecanumControllerCommand(
+ exampleTrajectory, [this]() { return m_drive.GetPose(); },
+
+ frc::SimpleMotorFeedforward<units::meters>(ks, kv, ka),
+ DriveConstants::kDriveKinematics,
+
+ frc2::PIDController(AutoConstants::kPXController, 0, 0),
+ frc2::PIDController(AutoConstants::kPYController, 0, 0),
+ frc::ProfiledPIDController<units::radians>(
+ AutoConstants::kPThetaController, 0, 0,
+ AutoConstants::kThetaControllerConstraints),
+
+ AutoConstants::kMaxSpeed,
+
+ [this]() {
+ return frc::MecanumDriveWheelSpeeds{
+ units::meters_per_second_t(m_drive.GetFrontLeftEncoder().GetRate()),
+ units::meters_per_second_t(
+ m_drive.GetFrontRightEncoder().GetRate()),
+ units::meters_per_second_t(m_drive.GetRearLeftEncoder().GetRate()),
+ units::meters_per_second_t(
+ m_drive.GetRearRightEncoder().GetRate())};
+ },
+
+ frc2::PIDController(DriveConstants::kPFrontLeftVel, 0, 0),
+ frc2::PIDController(DriveConstants::kPRearLeftVel, 0, 0),
+ frc2::PIDController(DriveConstants::kPFrontRightVel, 0, 0),
+ frc2::PIDController(DriveConstants::kPRearRightVel, 0, 0),
+
+ [this](units::volt_t frontLeft, units::volt_t rearLeft,
+ units::volt_t frontRight, units::volt_t rearRight) {
+ m_drive.SetSpeedControllersVolts(frontLeft, rearLeft, frontRight,
+ rearRight);
+ },
+
+ {&m_drive});
+
+ // no auto
+ return new frc2::SequentialCommandGroup(
+ std::move(mecanumControllerCommand),
+ frc2::InstantCommand([this]() { m_drive.Drive(0, 0, 0, false); }, {}));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..b758b07
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+#include <frc/geometry/Rotation2d.h>
+#include <units/units.h>
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+ : m_frontLeft{kFrontLeftMotorPort},
+ m_rearLeft{kRearLeftMotorPort},
+ m_frontRight{kFrontRightMotorPort},
+ m_rearRight{kRearRightMotorPort},
+
+ m_frontLeftEncoder{kFrontLeftEncoderPorts[0], kFrontLeftEncoderPorts[1],
+ kFrontLeftEncoderReversed},
+ m_rearLeftEncoder{kRearLeftEncoderPorts[0], kRearLeftEncoderPorts[1],
+ kRearLeftEncoderReversed},
+ m_frontRightEncoder{kFrontRightEncoderPorts[0],
+ kFrontRightEncoderPorts[1],
+ kFrontRightEncoderReversed},
+ m_rearRightEncoder{kRearRightEncoderPorts[0], kRearRightEncoderPorts[1],
+ kRearRightEncoderReversed},
+
+ m_odometry{kDriveKinematics,
+ frc::Rotation2d(units::degree_t(GetHeading())),
+ frc::Pose2d()} {
+ // Set the distance per pulse for the encoders
+ m_frontLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ m_rearLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ m_frontRightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ m_rearRightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void DriveSubsystem::Periodic() {
+ // Implementation of subsystem periodic method goes here.
+ m_odometry.Update(
+ frc::Rotation2d(units::degree_t(GetHeading())),
+ frc::MecanumDriveWheelSpeeds{
+ units::meters_per_second_t(m_frontLeftEncoder.GetRate()),
+ units::meters_per_second_t(m_rearLeftEncoder.GetRate()),
+ units::meters_per_second_t(m_frontRightEncoder.GetRate()),
+ units::meters_per_second_t(m_rearRightEncoder.GetRate())});
+}
+
+void DriveSubsystem::Drive(double xSpeed, double ySpeed, double rot,
+ bool feildRelative) {
+ if (feildRelative) {
+ m_drive.DriveCartesian(ySpeed, xSpeed, rot, -m_gyro.GetAngle());
+ } else {
+ m_drive.DriveCartesian(ySpeed, xSpeed, rot);
+ }
+}
+
+void DriveSubsystem::SetSpeedControllersVolts(units::volt_t frontLeftPower,
+ units::volt_t rearLeftPower,
+ units::volt_t frontRightPower,
+ units::volt_t rearRightPower) {
+ m_frontLeft.SetVoltage(frontLeftPower);
+ m_rearLeft.SetVoltage(rearLeftPower);
+ m_frontRight.SetVoltage(frontRightPower);
+ m_rearRight.SetVoltage(rearRightPower);
+}
+
+void DriveSubsystem::ResetEncoders() {
+ m_frontLeftEncoder.Reset();
+ m_rearLeftEncoder.Reset();
+ m_frontRightEncoder.Reset();
+ m_rearRightEncoder.Reset();
+}
+
+frc::Encoder& DriveSubsystem::GetFrontLeftEncoder() {
+ return m_frontLeftEncoder;
+}
+
+frc::Encoder& DriveSubsystem::GetRearLeftEncoder() { return m_rearLeftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetFrontRightEncoder() {
+ return m_frontRightEncoder;
+}
+
+frc::Encoder& DriveSubsystem::GetRearRightEncoder() {
+ return m_rearRightEncoder;
+}
+
+frc::MecanumDriveWheelSpeeds DriveSubsystem::getCurrentWheelSpeeds() {
+ return (frc::MecanumDriveWheelSpeeds{
+ units::meters_per_second_t(m_frontLeftEncoder.GetRate()),
+ units::meters_per_second_t(m_rearLeftEncoder.GetRate()),
+ units::meters_per_second_t(m_frontRightEncoder.GetRate()),
+ units::meters_per_second_t(m_rearRightEncoder.GetRate())});
+}
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+ m_drive.SetMaxOutput(maxOutput);
+}
+
+double DriveSubsystem::GetHeading() {
+ return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+}
+
+void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); }
+
+double DriveSubsystem::GetTurnRate() {
+ return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.);
+}
+
+frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
+
+void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
+ m_odometry.ResetPosition(pose,
+ frc::Rotation2d(units::degree_t(GetHeading())));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
new file mode 100644
index 0000000..5f5f05f
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/MecanumDriveKinematics.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <units/units.h>
+#include <wpi/math>
+
+#pragma once
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants. This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+constexpr int kFrontLeftMotorPort = 0;
+constexpr int kRearLeftMotorPort = 1;
+constexpr int kFrontRightMotorPort = 2;
+constexpr int kRearRightMotorPort = 3;
+
+constexpr int kFrontLeftEncoderPorts[]{0, 1};
+constexpr int kRearLeftEncoderPorts[]{2, 3};
+constexpr int kFrontRightEncoderPorts[]{4, 5};
+constexpr int kRearRightEncoderPorts[]{5, 6};
+
+constexpr bool kFrontLeftEncoderReversed = false;
+constexpr bool kRearLeftEncoderReversed = true;
+constexpr bool kFrontRightEncoderReversed = false;
+constexpr bool kRearRightEncoderReversed = true;
+
+constexpr auto kTrackWidth =
+ 0.5_m; // Distance between centers of right and left wheels on robot
+constexpr auto kWheelBase =
+ 0.7_m; // Distance between centers of front and back wheels on robot
+extern const frc::MecanumDriveKinematics kDriveKinematics;
+
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterMeters = .15;
+constexpr double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterMeters * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+
+constexpr bool kGyroReversed = false;
+
+// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+// These characterization values MUST be determined either experimentally or
+// theoretically for *your* robot's drive. The RobotPy Characterization
+// Toolsuite provides a convenient tool for obtaining these values for your
+// robot.
+constexpr auto ks = 1_V;
+constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
+constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m;
+
+// Example value only - as above, this must be tuned for your drive!
+constexpr double kPFrontLeftVel = 0.5;
+constexpr double kPRearLeftVel = 0.5;
+constexpr double kPFrontRightVel = 0.5;
+constexpr double kPRearRightVel = 0.5;
+} // namespace DriveConstants
+
+namespace AutoConstants {
+using radians_per_second_squared_t =
+ units::compound_unit<units::radians,
+ units::inverse<units::squared<units::second>>>;
+
+constexpr auto kMaxSpeed = units::meters_per_second_t(3);
+constexpr auto kMaxAcceleration = units::meters_per_second_squared_t(3);
+constexpr auto kMaxAngularSpeed = units::radians_per_second_t(3);
+constexpr auto kMaxAngularAcceleration =
+ units::unit_t<radians_per_second_squared_t>(3);
+
+constexpr double kPXController = 0.5;
+constexpr double kPYController = 0.5;
+constexpr double kPThetaController = 0.5;
+
+constexpr frc::TrapezoidProfile<units::radians>::Constraints
+ kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration};
+
+} // namespace AutoConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h
similarity index 60%
copy from wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
copy to wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h
index 2c70b8f..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -7,15 +7,17 @@
#pragma once
-#include <string>
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
-#include <frc/IterativeRobot.h>
-#include <frc/smartdashboard/SendableChooser.h>
+#include "RobotContainer.h"
-class Robot : public frc::IterativeRobot {
+class Robot : public frc::TimedRobot {
public:
void RobotInit() override;
void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
void AutonomousInit() override;
void AutonomousPeriodic() override;
void TeleopInit() override;
@@ -23,8 +25,9 @@
void TestPeriodic() override;
private:
- frc::SendableChooser<std::string> m_chooser;
- const std::string kAutoNameDefault = "Default";
- const std::string kAutoNameCustom = "My Auto";
- std::string m_autoSelected;
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h
new file mode 100644
index 0000000..3b4f6cc
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/PIDCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+
+#include "Constants.h"
+#include "subsystems/DriveSubsystem.h"
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The driver's controller
+ frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+ // The robot's subsystems and commands are defined here...
+
+ // The robot's subsystems
+ DriveSubsystem m_drive;
+
+ frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+ {}};
+ frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
+ {}};
+
+ // The chooser for the autonomous routines
+ frc::SendableChooser<frc2::Command*> m_chooser;
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..fb3c19d
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,166 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/ADXRS450_Gyro.h>
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/drive/MecanumDrive.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/interfaces/Gyro.h>
+#include <frc/kinematics/MecanumDriveOdometry.h>
+#include <frc/kinematics/MecanumDriveWheelSpeeds.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+ DriveSubsystem();
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic() override;
+
+ // Subsystem methods go here.
+
+ /**
+ * Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1]
+ * and the linear speeds have no effect on the angular speed.
+ *
+ * @param xSpeed Speed of the robot in the x direction
+ * (forward/backwards).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
+ * @param fieldRelative Whether the provided x and y speeds are relative to
+ * the field.
+ */
+ void Drive(double xSpeed, double ySpeed, double rot, bool feildRelative);
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ void ResetEncoders();
+
+ /**
+ * Gets the front left drive encoder.
+ *
+ * @return the front left drive encoder
+ */
+ frc::Encoder& GetFrontLeftEncoder();
+
+ /**
+ * Gets the rear left drive encoder.
+ *
+ * @return the rear left drive encoder
+ */
+ frc::Encoder& GetRearLeftEncoder();
+
+ /**
+ * Gets the front right drive encoder.
+ *
+ * @return the front right drive encoder
+ */
+ frc::Encoder& GetFrontRightEncoder();
+
+ /**
+ * Gets the rear right drive encoder.
+ *
+ * @return the rear right drive encoder
+ */
+ frc::Encoder& GetRearRightEncoder();
+
+ /**
+ * Gets the wheel speeds.
+ *
+ * @return the current wheel speeds.
+ */
+ frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds();
+
+ /**
+ * Sets the drive SpeedControllers to a desired voltage.
+ */
+ void SetSpeedControllersVolts(units::volt_t frontLeftPower,
+ units::volt_t rearLeftPower,
+ units::volt_t frontRightPower,
+ units::volt_t rearRightPower);
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive
+ * more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ void SetMaxOutput(double maxOutput);
+
+ /**
+ * Returns the heading of the robot.
+ *
+ * @return the robot's heading in degrees, from 180 to 180
+ */
+ double GetHeading();
+
+ /**
+ * Zeroes the heading of the robot.
+ */
+ void ZeroHeading();
+
+ /**
+ * Returns the turn rate of the robot.
+ *
+ * @return The turn rate of the robot, in degrees per second
+ */
+ double GetTurnRate();
+
+ /**
+ * Returns the currently-estimated pose of the robot.
+ *
+ * @return The pose.
+ */
+ frc::Pose2d GetPose();
+
+ /**
+ * Resets the odometry to the specified pose.
+ *
+ * @param pose The pose to which to set the odometry.
+ */
+ void ResetOdometry(frc::Pose2d pose);
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+
+ // The motor controllers
+ frc::PWMVictorSPX m_frontLeft;
+ frc::PWMVictorSPX m_rearLeft;
+ frc::PWMVictorSPX m_frontRight;
+ frc::PWMVictorSPX m_rearRight;
+
+ // The robot's drive
+ frc::MecanumDrive m_drive{m_frontLeft, m_rearLeft, m_frontRight, m_rearRight};
+
+ // The front-left-side drive encoder
+ frc::Encoder m_frontLeftEncoder;
+
+ // The rear-left-side drive encoder
+ frc::Encoder m_rearLeftEncoder;
+
+ // The front-right--side drive encoder
+ frc::Encoder m_frontRightEncoder;
+
+ // The rear-right-side drive encoder
+ frc::Encoder m_rearRightEncoder;
+
+ // The gyro sensor
+ frc::ADXRS450_Gyro m_gyro;
+
+ // Odometry class for tracking robot pose
+ frc::MecanumDriveOdometry m_odometry;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
index c5d22d0..71da410 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
@@ -12,7 +12,7 @@
#include <frc/shuffleboard/Shuffleboard.h>
#include <frc/trajectory/Trajectory.h>
#include <frc/trajectory/TrajectoryGenerator.h>
-#include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
+#include <frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h>
#include <frc2/command/InstantCommand.h>
#include <frc2/command/RamseteCommand.h>
#include <frc2/command/SequentialCommandGroup.h>
@@ -44,11 +44,19 @@
}
frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Create a voltage constraint to ensure we don't accelerate too fast
+ frc::DifferentialDriveVoltageConstraint autoVoltageConstraint(
+ frc::SimpleMotorFeedforward<units::meters>(
+ DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
+ DriveConstants::kDriveKinematics, 10_V);
+
// Set up config for trajectory
frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
AutoConstants::kMaxAcceleration);
// Add kinematics to ensure max speed is actually obeyed
config.SetKinematics(DriveConstants::kDriveKinematics);
+ // Apply the voltage constraint
+ config.AddConstraint(autoVoltageConstraint);
// An example trajectory to follow. All units in meters.
auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
@@ -65,23 +73,17 @@
exampleTrajectory, [this]() { return m_drive.GetPose(); },
frc::RamseteController(AutoConstants::kRamseteB,
AutoConstants::kRamseteZeta),
- DriveConstants::ks, DriveConstants::kv, DriveConstants::ka,
+ frc::SimpleMotorFeedforward<units::meters>(
+ DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
DriveConstants::kDriveKinematics,
- [this] {
- return units::meters_per_second_t(m_drive.GetLeftEncoder().GetRate());
- },
- [this] {
- return units::meters_per_second_t(m_drive.GetRightEncoder().GetRate());
- },
+ [this] { return m_drive.GetWheelSpeeds(); },
frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
- [this](auto left, auto right) {
- m_drive.TankDrive(left / 12_V, right / 12_V);
- },
+ [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
{&m_drive});
// no auto
return new frc2::SequentialCommandGroup(
std::move(ramseteCommand),
- frc2::InstantCommand([this] { m_drive.TankDrive(0, 0); }, {}));
+ frc2::InstantCommand([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
}
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
index 3d5307f..be5f82a 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -7,8 +7,6 @@
#include "subsystems/DriveSubsystem.h"
-#include <units/units.h>
-
#include <frc/geometry/Rotation2d.h>
#include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
@@ -21,26 +19,28 @@
m_right2{kRightMotor2Port},
m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
- m_odometry{kDriveKinematics, frc::Pose2d()} {
+ m_odometry{frc::Rotation2d(units::degree_t(GetHeading()))} {
// Set the distance per pulse for the encoders
m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+
+ ResetEncoders();
}
void DriveSubsystem::Periodic() {
// Implementation of subsystem periodic method goes here.
m_odometry.Update(frc::Rotation2d(units::degree_t(GetHeading())),
- frc::DifferentialDriveWheelSpeeds{
- units::meters_per_second_t(m_leftEncoder.GetRate()),
- units::meters_per_second_t(m_rightEncoder.GetRate())});
+ units::meter_t(m_leftEncoder.GetDistance()),
+ units::meter_t(m_rightEncoder.GetDistance()));
}
void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
m_drive.ArcadeDrive(fwd, rot);
}
-void DriveSubsystem::TankDrive(double left, double right) {
- m_drive.TankDrive(left, right, false);
+void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
+ m_leftMotors.SetVoltage(left);
+ m_rightMotors.SetVoltage(-right);
}
void DriveSubsystem::ResetEncoders() {
@@ -49,7 +49,7 @@
}
double DriveSubsystem::GetAverageEncoderDistance() {
- return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
}
frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
@@ -61,15 +61,22 @@
}
double DriveSubsystem::GetHeading() {
- return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+ return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
}
double DriveSubsystem::GetTurnRate() {
- return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.);
+ return m_gyro.GetRate() * (kGyroReversed ? -1.0 : 1.0);
}
frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
+frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
+ return {units::meters_per_second_t(m_leftEncoder.GetRate()),
+ units::meters_per_second_t(m_rightEncoder.GetRate())};
+}
+
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
- m_odometry.ResetPosition(pose);
+ ResetEncoders();
+ m_odometry.ResetPosition(pose,
+ frc::Rotation2d(units::degree_t(GetHeading())));
}
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
index 801e479..22564c6 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
@@ -5,10 +5,10 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include <units/units.h>
-
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
+#include <units/units.h>
+#include <wpi/math>
#pragma once
@@ -22,50 +22,50 @@
*/
namespace DriveConstants {
-const int kLeftMotor1Port = 0;
-const int kLeftMotor2Port = 1;
-const int kRightMotor1Port = 2;
-const int kRightMotor2Port = 3;
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
-const int kLeftEncoderPorts[]{0, 1};
-const int kRightEncoderPorts[]{2, 3};
-const bool kLeftEncoderReversed = false;
-const bool kRightEncoderReversed = true;
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
-const auto kTrackwidth = .6_m;
-const frc::DifferentialDriveKinematics kDriveKinematics(kTrackwidth);
+constexpr auto kTrackwidth = 0.6_m;
+constexpr frc::DifferentialDriveKinematics kDriveKinematics(kTrackwidth);
-const int kEncoderCPR = 1024;
-const double kWheelDiameterInches = 6;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
- (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+ (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
-const bool kGyroReversed = true;
+constexpr bool kGyroReversed = true;
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
// These characterization values MUST be determined either experimentally or
// theoretically for *your* robot's drive. The RobotPy Characterization
// Toolsuite provides a convenient tool for obtaining these values for your
// robot.
-const auto ks = 1_V;
-const auto kv = .8 * 1_V * 1_s / 1_m;
-const auto ka = .15 * 1_V * 1_s * 1_s / 1_m;
+constexpr auto ks = 1_V;
+constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
+constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m;
// Example value only - as above, this must be tuned for your drive!
-const double kPDriveVel = .5;
+constexpr double kPDriveVel = 0.5;
} // namespace DriveConstants
namespace AutoConstants {
-const auto kMaxSpeed = 3_mps;
-const auto kMaxAcceleration = 3_mps_sq;
+constexpr auto kMaxSpeed = 3_mps;
+constexpr auto kMaxAcceleration = 3_mps_sq;
// Reasonable baseline values for a RAMSETE follower in units of meters and
// seconds
-const double kRamseteB = 2;
-const double kRamseteZeta = .7;
+constexpr double kRamseteB = 2;
+constexpr double kRamseteZeta = 0.7;
} // namespace AutoConstants
namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h
index cc91e0d..d6e4288 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h
@@ -41,7 +41,7 @@
// The robot's subsystems
DriveSubsystem m_drive;
- frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+ frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
{}};
frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
{}};
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
index 620cfd8..d8d1abd 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
@@ -15,6 +15,7 @@
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/DifferentialDriveOdometry.h>
#include <frc2/command/SubsystemBase.h>
+#include <units/units.h>
#include "Constants.h"
@@ -38,13 +39,12 @@
void ArcadeDrive(double fwd, double rot);
/**
- * Drives the robot using tank controls. Does not square inputs to enable
- * composition with external controllers.
+ * Controls each side of the drive directly with a voltage.
*
* @param left the commanded left output
* @param right the commanded right output
*/
- void TankDrive(double left, double right);
+ void TankDriveVolts(units::volt_t left, units::volt_t right);
/**
* Resets the drive encoders to currently read a position of 0.
@@ -102,6 +102,13 @@
frc::Pose2d GetPose();
/**
+ * Returns the current wheel speeds of the robot.
+ *
+ * @return The current wheel speeds.
+ */
+ frc::DifferentialDriveWheelSpeeds GetWheelSpeeds();
+
+ /**
* Resets the odometry to the specified pose.
*
* @param pose The pose to which to set the odometry.
diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h
index 0a5f832..9fb2281 100644
--- a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h
@@ -18,5 +18,5 @@
*/
namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
index 0a5f832..9fb2281 100644
--- a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
@@ -18,5 +18,5 @@
*/
namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
index 697984c..f7a11b8 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
@@ -26,7 +26,8 @@
// Limit the PID Controller's input range between -pi and pi and set the input
// to be continuous.
- m_turningPIDController.EnableContinuousInput(-wpi::math::pi, wpi::math::pi);
+ m_turningPIDController.EnableContinuousInput(-units::radian_t(wpi::math::pi),
+ units::radian_t(wpi::math::pi));
}
frc::SwerveModuleState SwerveModule::GetState() const {
@@ -41,10 +42,7 @@
// Calculate the turning motor output from the turning PID controller.
const auto turnOutput = m_turningPIDController.Calculate(
- units::meter_t(m_turningEncoder.Get()),
- // We have to convert to the meters type here because that's what
- // ProfiledPIDController wants.
- units::meter_t(state.angle.Radians().to<double>()));
+ units::radian_t(m_turningEncoder.Get()), state.angle.Radians());
// Set the motor outputs.
m_driveMotor.Set(driveOutput);
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
index 745581a..2de6e95 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
@@ -57,5 +57,5 @@
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
m_backRightLocation};
- frc::SwerveDriveOdometry<4> m_odometry{m_kinematics};
+ frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, GetAngle()};
};
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
index 0eaa69e..0dafa24 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
@@ -8,7 +8,7 @@
#pragma once
#include <frc/Encoder.h>
-#include <frc/Spark.h>
+#include <frc/PWMVictorSPX.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/kinematics/SwerveModuleState.h>
@@ -24,24 +24,19 @@
static constexpr double kWheelRadius = 0.0508;
static constexpr int kEncoderResolution = 4096;
- // We have to use meters here instead of radians because of the fact that
- // ProfiledPIDController's constraints only take in meters per second and
- // meters per second squared.
+ static constexpr auto kModuleMaxAngularVelocity =
+ wpi::math::pi * 1_rad_per_s; // radians per second
+ static constexpr auto kModuleMaxAngularAcceleration =
+ wpi::math::pi * 2_rad_per_s / 1_s; // radians per second^2
- static constexpr units::meters_per_second_t kModuleMaxAngularVelocity =
- units::meters_per_second_t(wpi::math::pi); // radians per second
- static constexpr units::meters_per_second_squared_t
- kModuleMaxAngularAcceleration = units::meters_per_second_squared_t(
- wpi::math::pi * 2.0); // radians per second squared
-
- frc::Spark m_driveMotor;
- frc::Spark m_turningMotor;
+ frc::PWMVictorSPX m_driveMotor;
+ frc::PWMVictorSPX m_turningMotor;
frc::Encoder m_driveEncoder{0, 1};
- frc::Encoder m_turningEncoder{0, 1};
+ frc::Encoder m_turningEncoder{2, 3};
frc2::PIDController m_drivePIDController{1.0, 0, 0};
- frc::ProfiledPIDController m_turningPIDController{
+ frc::ProfiledPIDController<units::radians> m_turningPIDController{
1.0,
0.0,
0.0,
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..44efba5
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/controller/PIDController.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc/trajectory/Trajectory.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/SwerveControllerCommand.h>
+#include <frc2/command/button/JoystickButton.h>
+#include <units/units.h>
+
+#include "Constants.h"
+#include "subsystems/DriveSubsystem.h"
+
+using namespace DriveConstants;
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+
+ // Set up default drive command
+ m_drive.SetDefaultCommand(frc2::RunCommand(
+ [this] {
+ m_drive.Drive(units::meters_per_second_t(
+ m_driverController.GetY(frc::GenericHID::kLeftHand)),
+ units::meters_per_second_t(
+ m_driverController.GetY(frc::GenericHID::kRightHand)),
+ units::radians_per_second_t(
+ m_driverController.GetX(frc::GenericHID::kLeftHand)),
+ false);
+ },
+ {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Set up config for trajectory
+ frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
+ AutoConstants::kMaxAcceleration);
+ // Add kinematics to ensure max speed is actually obeyed
+ config.SetKinematics(m_drive.kDriveKinematics);
+
+ // An example trajectory to follow. All units in meters.
+ auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ // Start at the origin facing the +X direction
+ frc::Pose2d(0_m, 0_m, frc::Rotation2d(0_deg)),
+ // Pass through these two interior waypoints, making an 's' curve path
+ {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
+ // End 3 meters straight ahead of where we started, facing forward
+ frc::Pose2d(3_m, 0_m, frc::Rotation2d(0_deg)),
+ // Pass the config
+ config);
+
+ frc2::SwerveControllerCommand<4> swerveControllerCommand(
+ exampleTrajectory, [this]() { return m_drive.GetPose(); },
+
+ m_drive.kDriveKinematics,
+
+ frc2::PIDController(AutoConstants::kPXController, 0, 0),
+ frc2::PIDController(AutoConstants::kPYController, 0, 0),
+ frc::ProfiledPIDController<units::radians>(
+ AutoConstants::kPThetaController, 0, 0,
+ AutoConstants::kThetaControllerConstraints),
+
+ [this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); },
+
+ {&m_drive});
+
+ // no auto
+ return new frc2::SequentialCommandGroup(
+ std::move(swerveControllerCommand), std::move(swerveControllerCommand),
+ frc2::InstantCommand(
+ [this]() {
+ m_drive.Drive(units::meters_per_second_t(0),
+ units::meters_per_second_t(0),
+ units::radians_per_second_t(0), false);
+ },
+ {}));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..f5d05ce
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+#include <frc/geometry/Rotation2d.h>
+#include <units/units.h>
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+ : m_frontLeft{kFrontLeftDriveMotorPort,
+ kFrontLeftTurningMotorPort,
+ kFrontLeftDriveEncoderPorts,
+ kFrontLeftTurningEncoderPorts,
+ kFrontLeftDriveEncoderReversed,
+ kFrontLeftTurningEncoderReversed},
+
+ m_rearLeft{
+ kRearLeftDriveMotorPort, kRearLeftTurningMotorPort,
+ kRearLeftDriveEncoderPorts, kRearLeftTurningEncoderPorts,
+ kRearLeftDriveEncoderReversed, kRearLeftTurningEncoderReversed},
+
+ m_frontRight{
+ kFrontRightDriveMotorPort, kFrontRightTurningMotorPort,
+ kFrontRightDriveEncoderPorts, kFrontRightTurningEncoderPorts,
+ kFrontRightDriveEncoderReversed, kFrontRightTurningEncoderReversed},
+
+ m_rearRight{
+ kRearRightDriveMotorPort, kRearRightTurningMotorPort,
+ kRearRightDriveEncoderPorts, kRearRightTurningEncoderPorts,
+ kRearRightDriveEncoderReversed, kRearRightTurningEncoderReversed},
+
+ m_odometry{kDriveKinematics,
+ frc::Rotation2d(units::degree_t(GetHeading())),
+ frc::Pose2d()} {}
+
+void DriveSubsystem::Periodic() {
+ // Implementation of subsystem periodic method goes here.
+ m_odometry.Update(frc::Rotation2d(units::degree_t(GetHeading())),
+ m_frontLeft.GetState(), m_rearLeft.GetState(),
+ m_frontRight.GetState(), m_rearRight.GetState());
+}
+
+void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
+ units::meters_per_second_t ySpeed,
+ units::radians_per_second_t rot,
+ bool fieldRelative) {
+ auto states = kDriveKinematics.ToSwerveModuleStates(
+ fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+ xSpeed, ySpeed, rot,
+ frc::Rotation2d(units::degree_t(GetHeading())))
+ : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
+
+ kDriveKinematics.NormalizeWheelSpeeds(&states, AutoConstants::kMaxSpeed);
+
+ auto [fl, fr, bl, br] = states;
+
+ m_frontLeft.SetDesiredState(fl);
+ m_frontRight.SetDesiredState(fr);
+ m_rearLeft.SetDesiredState(bl);
+ m_rearRight.SetDesiredState(br);
+}
+
+void DriveSubsystem::SetModuleStates(
+ std::array<frc::SwerveModuleState, 4> desiredStates) {
+ kDriveKinematics.NormalizeWheelSpeeds(&desiredStates,
+ AutoConstants::kMaxSpeed);
+ m_frontLeft.SetDesiredState(desiredStates[0]);
+ m_rearLeft.SetDesiredState(desiredStates[1]);
+ m_frontRight.SetDesiredState(desiredStates[2]);
+ m_rearRight.SetDesiredState(desiredStates[3]);
+}
+
+void DriveSubsystem::ResetEncoders() {
+ m_frontLeft.ResetEncoders();
+ m_rearLeft.ResetEncoders();
+ m_frontRight.ResetEncoders();
+ m_rearRight.ResetEncoders();
+}
+
+double DriveSubsystem::GetHeading() {
+ return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+}
+
+void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); }
+
+double DriveSubsystem::GetTurnRate() {
+ return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.);
+}
+
+frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
+
+void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
+ m_odometry.ResetPosition(pose,
+ frc::Rotation2d(units::degree_t(GetHeading())));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp
new file mode 100644
index 0000000..22b3f0d
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/SwerveModule.h"
+
+#include <frc/geometry/Rotation2d.h>
+#include <wpi/math>
+
+#include "Constants.h"
+
+SwerveModule::SwerveModule(int driveMotorChannel, int turningMotorChannel,
+ const int driveEncoderPorts[],
+ const int turningEncoderPorts[],
+ bool driveEncoderReversed,
+ bool turningEncoderReversed)
+ : m_driveMotor(driveMotorChannel),
+ m_turningMotor(turningMotorChannel),
+ m_driveEncoder(driveEncoderPorts[0], driveEncoderPorts[1]),
+ m_turningEncoder(turningEncoderPorts[0], turningEncoderPorts[1]),
+ m_reverseDriveEncoder(driveEncoderReversed),
+ m_reverseTurningEncoder(turningEncoderReversed) {
+ // Set the distance per pulse for the drive encoder. We can simply use the
+ // distance traveled for one rotation of the wheel divided by the encoder
+ // resolution.
+ m_driveEncoder.SetDistancePerPulse(
+ ModuleConstants::kDriveEncoderDistancePerPulse);
+
+ // Set the distance (in this case, angle) per pulse for the turning encoder.
+ // This is the the angle through an entire rotation (2 * wpi::math::pi)
+ // divided by the encoder resolution.
+ m_turningEncoder.SetDistancePerPulse(
+ ModuleConstants::kTurningEncoderDistancePerPulse);
+
+ // Limit the PID Controller's input range between -pi and pi and set the input
+ // to be continuous.
+ m_turningPIDController.EnableContinuousInput(units::radian_t(-wpi::math::pi),
+ units::radian_t(wpi::math::pi));
+}
+
+frc::SwerveModuleState SwerveModule::GetState() {
+ return {units::meters_per_second_t{m_driveEncoder.GetRate()},
+ frc::Rotation2d(units::radian_t(m_turningEncoder.Get()))};
+}
+
+void SwerveModule::SetDesiredState(frc::SwerveModuleState& state) {
+ // Calculate the drive output from the drive PID controller.
+ const auto driveOutput = m_drivePIDController.Calculate(
+ m_driveEncoder.GetRate(), state.speed.to<double>());
+
+ // Calculate the turning motor output from the turning PID controller.
+ auto turnOutput = m_turningPIDController.Calculate(
+ units::radian_t(m_turningEncoder.Get()), state.angle.Radians());
+
+ // Set the motor outputs.
+ m_driveMotor.Set(driveOutput);
+ m_turningMotor.Set(turnOutput);
+}
+
+void SwerveModule::ResetEncoders() {
+ m_driveEncoder.Reset();
+ m_turningEncoder.Reset();
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
new file mode 100644
index 0000000..ed1f880
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <units/units.h>
+#include <wpi/math>
+
+#pragma once
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants. This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+constexpr int kFrontLeftDriveMotorPort = 0;
+constexpr int kRearLeftDriveMotorPort = 2;
+constexpr int kFrontRightDriveMotorPort = 4;
+constexpr int kRearRightDriveMotorPort = 6;
+
+constexpr int kFrontLeftTurningMotorPort = 1;
+constexpr int kRearLeftTurningMotorPort = 3;
+constexpr int kFrontRightTurningMotorPort = 5;
+constexpr int kRearRightTurningMotorPort = 7;
+
+constexpr int kFrontLeftTurningEncoderPorts[2]{0, 1};
+constexpr int kRearLeftTurningEncoderPorts[2]{2, 3};
+constexpr int kFrontRightTurningEncoderPorts[2]{4, 5};
+constexpr int kRearRightTurningEncoderPorts[2]{5, 6};
+
+constexpr bool kFrontLeftTurningEncoderReversed = false;
+constexpr bool kRearLeftTurningEncoderReversed = true;
+constexpr bool kFrontRightTurningEncoderReversed = false;
+constexpr bool kRearRightTurningEncoderReversed = true;
+
+constexpr int kFrontLeftDriveEncoderPorts[2]{0, 1};
+constexpr int kRearLeftDriveEncoderPorts[2]{2, 3};
+constexpr int kFrontRightDriveEncoderPorts[2]{4, 5};
+constexpr int kRearRightDriveEncoderPorts[2]{5, 6};
+
+constexpr bool kFrontLeftDriveEncoderReversed = false;
+constexpr bool kRearLeftDriveEncoderReversed = true;
+constexpr bool kFrontRightDriveEncoderReversed = false;
+constexpr bool kRearRightDriveEncoderReversed = true;
+
+constexpr bool kGyroReversed = false;
+
+// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+// These characterization values MUST be determined either experimentally or
+// theoretically for *your* robot's drive. The RobotPy Characterization
+// Toolsuite provides a convenient tool for obtaining these values for your
+// robot.
+constexpr auto ks = 1_V;
+constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
+constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m;
+
+// Example value only - as above, this must be tuned for your drive!
+constexpr double kPFrontLeftVel = 0.5;
+constexpr double kPRearLeftVel = 0.5;
+constexpr double kPFrontRightVel = 0.5;
+constexpr double kPRearRightVel = 0.5;
+} // namespace DriveConstants
+
+namespace ModuleConstants {
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterMeters = .15;
+constexpr double kDriveEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterMeters * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+
+constexpr double kTurningEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (wpi::math::pi * 2) / static_cast<double>(kEncoderCPR);
+
+constexpr double kPModuleTurningController = 1;
+constexpr double kPModuleDriveController = 1;
+} // namespace ModuleConstants
+
+namespace AutoConstants {
+using radians_per_second_squared_t =
+ units::compound_unit<units::radians,
+ units::inverse<units::squared<units::second>>>;
+
+constexpr auto kMaxSpeed = units::meters_per_second_t(3);
+constexpr auto kMaxAcceleration = units::meters_per_second_squared_t(3);
+constexpr auto kMaxAngularSpeed = units::radians_per_second_t(3.142);
+constexpr auto kMaxAngularAcceleration =
+ units::unit_t<radians_per_second_squared_t>(3.142);
+
+constexpr double kPXController = 0.5;
+constexpr double kPYController = 0.5;
+constexpr double kPThetaController = 0.5;
+
+//
+
+constexpr frc::TrapezoidProfile<units::radians>::Constraints
+ kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration};
+
+} // namespace AutoConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h
similarity index 60%
copy from wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
copy to wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h
index 2c70b8f..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -7,15 +7,17 @@
#pragma once
-#include <string>
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
-#include <frc/IterativeRobot.h>
-#include <frc/smartdashboard/SendableChooser.h>
+#include "RobotContainer.h"
-class Robot : public frc::IterativeRobot {
+class Robot : public frc::TimedRobot {
public:
void RobotInit() override;
void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
void AutonomousInit() override;
void AutonomousPeriodic() override;
void TeleopInit() override;
@@ -23,8 +25,9 @@
void TestPeriodic() override;
private:
- frc::SendableChooser<std::string> m_chooser;
- const std::string kAutoNameDefault = "Default";
- const std::string kAutoNameCustom = "My Auto";
- std::string m_autoSelected;
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h
new file mode 100644
index 0000000..8b36617
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/PIDCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+
+#include "Constants.h"
+#include "subsystems/DriveSubsystem.h"
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The driver's controller
+ frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+ // The robot's subsystems and commands are defined here...
+
+ // The robot's subsystems
+ DriveSubsystem m_drive;
+
+ // The chooser for the autonomous routines
+ frc::SendableChooser<frc2::Command*> m_chooser;
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..db4623f
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/ADXRS450_Gyro.h>
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/drive/MecanumDrive.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/interfaces/Gyro.h>
+#include <frc/kinematics/ChassisSpeeds.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/kinematics/SwerveDriveOdometry.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+#include "SwerveModule.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+ DriveSubsystem();
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic() override;
+
+ // Subsystem methods go here.
+
+ /**
+ * Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1]
+ * and the linear speeds have no effect on the angular speed.
+ *
+ * @param xSpeed Speed of the robot in the x direction
+ * (forward/backwards).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
+ * @param fieldRelative Whether the provided x and y speeds are relative to
+ * the field.
+ */
+ void Drive(units::meters_per_second_t xSpeed,
+ units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
+ bool feildRelative);
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ void ResetEncoders();
+
+ /**
+ * Sets the drive SpeedControllers to a power from -1 to 1.
+ */
+ void SetModuleStates(std::array<frc::SwerveModuleState, 4> desiredStates);
+
+ /**
+ * Returns the heading of the robot.
+ *
+ * @return the robot's heading in degrees, from 180 to 180
+ */
+ double GetHeading();
+
+ /**
+ * Zeroes the heading of the robot.
+ */
+ void ZeroHeading();
+
+ /**
+ * Returns the turn rate of the robot.
+ *
+ * @return The turn rate of the robot, in degrees per second
+ */
+ double GetTurnRate();
+
+ /**
+ * Returns the currently-estimated pose of the robot.
+ *
+ * @return The pose.
+ */
+ frc::Pose2d GetPose();
+
+ /**
+ * Resets the odometry to the specified pose.
+ *
+ * @param pose The pose to which to set the odometry.
+ */
+ void ResetOdometry(frc::Pose2d pose);
+
+ units::meter_t kTrackWidth =
+ .5_m; // Distance between centers of right and left wheels on robot
+ units::meter_t kWheelBase =
+ .7_m; // Distance between centers of front and back wheels on robot
+
+ frc::SwerveDriveKinematics<4> kDriveKinematics{
+ frc::Translation2d(kWheelBase / 2, kTrackWidth / 2),
+ frc::Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+ frc::Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+ frc::Translation2d(-kWheelBase / 2, -kTrackWidth / 2)};
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+
+ SwerveModule m_frontLeft;
+ SwerveModule m_rearLeft;
+ SwerveModule m_frontRight;
+ SwerveModule m_rearRight;
+
+ // The gyro sensor
+ frc::ADXRS450_Gyro m_gyro;
+
+ // Odometry class for tracking robot pose
+ // 4 defines the number of modules
+ frc::SwerveDriveOdometry<4> m_odometry;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h
new file mode 100644
index 0000000..f8d816a
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/Spark.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <wpi/math>
+
+#include "Constants.h"
+
+class SwerveModule {
+ using radians_per_second_squared_t =
+ units::compound_unit<units::radians,
+ units::inverse<units::squared<units::second>>>;
+
+ public:
+ SwerveModule(int driveMotorChannel, int turningMotorChannel,
+ const int driveEncoderPorts[2], const int turningEncoderPorts[2],
+ bool driveEncoderReversed, bool turningEncoderReversed);
+
+ frc::SwerveModuleState GetState();
+
+ void SetDesiredState(frc::SwerveModuleState& state);
+
+ void ResetEncoders();
+
+ private:
+ // We have to use meters here instead of radians due to the fact that
+ // ProfiledPIDController's constraints only take in meters per second and
+ // meters per second squared.
+
+ static constexpr units::radians_per_second_t kModuleMaxAngularVelocity =
+ units::radians_per_second_t(wpi::math::pi); // radians per second
+ static constexpr units::unit_t<radians_per_second_squared_t>
+ kModuleMaxAngularAcceleration =
+ units::unit_t<radians_per_second_squared_t>(
+ wpi::math::pi * 2.0); // radians per second squared
+
+ frc::Spark m_driveMotor;
+ frc::Spark m_turningMotor;
+
+ frc::Encoder m_driveEncoder;
+ frc::Encoder m_turningEncoder;
+
+ bool m_reverseDriveEncoder;
+ bool m_reverseTurningEncoder;
+
+ frc2::PIDController m_drivePIDController{
+ ModuleConstants::kPModuleDriveController, 0, 0};
+ frc::ProfiledPIDController<units::radians> m_turningPIDController{
+ ModuleConstants::kPModuleTurningController,
+ 0.0,
+ 0.0,
+ {kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp
new file mode 100644
index 0000000..3ca31ed
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/GenericHID.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/drive/DifferentialDrive.h>
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class.
+ * Runs the motors with tank steering and an Xbox controller.
+ */
+class Robot : public frc::TimedRobot {
+ frc::PWMVictorSPX m_leftMotor{0};
+ frc::PWMVictorSPX m_rightMotor{1};
+ frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
+ frc::XboxController m_driverController{0};
+
+ public:
+ void TeleopPeriodic() {
+ // Drive with tank style
+ m_robotDrive.TankDrive(
+ m_driverController.GetY(frc::GenericHID::JoystickHand::kLeftHand),
+ m_driverController.GetY(frc::GenericHID::JoystickHand::kRightHand));
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
index 2cc2131..9f9c01d 100644
--- a/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
@@ -1,11 +1,12 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
/*----------------------------------------------------------------------------*/
#include <frc/AnalogInput.h>
+#include <frc/MedianFilter.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/drive/DifferentialDrive.h>
@@ -22,7 +23,10 @@
*/
void TeleopPeriodic() override {
// Sensor returns a value from 0-4095 that is scaled to inches
- double currentDistance = m_ultrasonic.GetValue() * kValueToInches;
+ // returned value is filtered with a rolling median filter, since
+ // ultrasonics tend to be quite noisy and susceptible to sudden outliers
+ double currentDistance =
+ m_filter.Calculate(m_ultrasonic.GetVoltage()) * kValueToInches;
// Convert distance error to a motor speed
double currentSpeed = (kHoldDistance - currentDistance) * kP;
// Drive robot
@@ -43,6 +47,9 @@
static constexpr int kRightMotorPort = 1;
static constexpr int kUltrasonicPort = 0;
+ // median filter to discard outliers; filters over 10 samples
+ frc::MedianFilter<double> m_filter{10};
+
frc::AnalogInput m_ultrasonic{kUltrasonicPort};
frc::PWMVictorSPX m_left{kLeftMotorPort};
diff --git a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
index 745fc10..47bd62d 100644
--- a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
@@ -6,6 +6,7 @@
/*----------------------------------------------------------------------------*/
#include <frc/AnalogInput.h>
+#include <frc/MedianFilter.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/controller/PIDController.h>
@@ -27,7 +28,8 @@
}
void TeleopPeriodic() override {
- double output = m_pidController.Calculate(m_ultrasonic.GetAverageVoltage());
+ double output =
+ m_pidController.Calculate(m_filter.Calculate(m_ultrasonic.GetValue()));
m_robotDrive.ArcadeDrive(output, 0);
}
@@ -51,6 +53,9 @@
static constexpr int kRightMotorPort = 1;
static constexpr int kUltrasonicPort = 0;
+ // median filter to discard outliers; filters over 5 samples
+ frc::MedianFilter<double> m_filter{5};
+
frc::AnalogInput m_ultrasonic{kUltrasonicPort};
frc::PWMVictorSPX m_left{kLeftMotorPort};
diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json
index 864ea1d..4aa9f0e 100644
--- a/wpilibcExamples/src/main/cpp/examples/examples.json
+++ b/wpilibcExamples/src/main/cpp/examples/examples.json
@@ -9,7 +9,8 @@
"Complete List"
],
"foldername": "MotorControl",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Motor Control With Encoder",
@@ -23,7 +24,8 @@
"Complete List"
],
"foldername": "MotorControlEncoder",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Relay",
@@ -34,7 +36,8 @@
"Complete List"
],
"foldername": "Relay",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "PDP CAN Monitoring",
@@ -45,7 +48,8 @@
"Sensors"
],
"foldername": "CANPDP",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Solenoids",
@@ -57,7 +61,8 @@
"Complete List"
],
"foldername": "Solenoid",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Encoder",
@@ -68,7 +73,8 @@
"Sensors"
],
"foldername": "Encoder",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Arcade Drive",
@@ -80,7 +86,8 @@
"Complete List"
],
"foldername": "ArcadeDrive",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Mecanum Drive",
@@ -92,7 +99,8 @@
"Complete List"
],
"foldername": "MecanumDrive",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Ultrasonic",
@@ -104,7 +112,8 @@
"Analog"
],
"foldername": "Ultrasonic",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "UltrasonicPID",
@@ -116,7 +125,8 @@
"Analog"
],
"foldername": "UltrasonicPID",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Gyro",
@@ -129,7 +139,8 @@
"Joystick"
],
"foldername": "Gyro",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Gyro Mecanum",
@@ -142,7 +153,8 @@
"Joysitck"
],
"foldername": "GyroMecanum",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "HID Rumble",
@@ -151,7 +163,8 @@
"Joystick"
],
"foldername": "HidRumble",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "PotentiometerPID",
@@ -164,7 +177,8 @@
"Analog"
],
"foldername": "PotentiometerPID",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Elevator with trapezoid profiled PID",
@@ -176,7 +190,8 @@
"Joystick"
],
"foldername": "ElevatorTrapezoidProfile",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Elevator with profiled PID controller",
@@ -188,7 +203,8 @@
"Joystick"
],
"foldername": "ElevatorProfiledPID",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Getting Started",
@@ -198,7 +214,8 @@
"Complete List"
],
"foldername": "GettingStarted",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Simple Vision",
@@ -208,7 +225,8 @@
"Complete List"
],
"foldername": "QuickVision",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Intermediate Vision",
@@ -218,7 +236,8 @@
"Complete List"
],
"foldername": "IntermediateVision",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Axis Camera Sample",
@@ -228,7 +247,8 @@
"Complete List"
],
"foldername": "AxisCameraSample",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "GearsBot",
@@ -238,7 +258,8 @@
"Complete List"
],
"foldername": "GearsBot",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "PacGoat",
@@ -248,7 +269,8 @@
"Complete List"
],
"foldername": "PacGoat",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "HAL",
@@ -257,7 +279,8 @@
"HAL"
],
"foldername": "HAL",
- "gradlebase": "c"
+ "gradlebase": "c",
+ "commandversion": 1
},
{
"name": "ShuffleBoard",
@@ -266,7 +289,8 @@
"ShuffleBoard"
],
"foldername": "ShuffleBoard",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "'Traditional' Hatchbot",
@@ -276,7 +300,8 @@
"Command-based"
],
"foldername": "HatchbotTraditional",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "'Inlined' Hatchbot",
@@ -287,7 +312,8 @@
"Lambdas"
],
"foldername": "HatchbotInlined",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "Select Command Example",
@@ -296,7 +322,8 @@
"Command-based"
],
"foldername": "SelectCommand",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "Scheduler Event Logging",
@@ -306,7 +333,8 @@
"Shuffleboard"
],
"foldername": "SchedulerEventLogging",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "Frisbeebot",
@@ -316,7 +344,8 @@
"PID"
],
"foldername": "Frisbeebot",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "Gyro Drive Commands",
@@ -327,7 +356,8 @@
"Gyro"
],
"foldername": "GyroDriveCommands",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "SwerveBot",
@@ -336,7 +366,8 @@
"SwerveBot"
],
"foldername": "SwerveBot",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "MecanumBot",
@@ -345,7 +376,8 @@
"MecanumBot"
],
"foldername": "MecanumBot",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "DifferentialDriveBot",
@@ -354,10 +386,11 @@
"DifferentialDriveBot"
],
"foldername": "DifferentialDriveBot",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
- "name:": "RamseteCommand",
+ "name": "RamseteCommand",
"description": "An example command-based robot demonstrating the use of a RamseteCommand to follow a pregenerated trajectory.",
"tags": [
"RamseteCommand",
@@ -367,6 +400,137 @@
"Path following"
],
"foldername": "RamseteCommand",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "Arcade Drive Xbox Controller",
+ "description": "An example program which demonstrates the use of Arcade Drive with the DifferentialDrive class and an Xbox Controller.",
+ "tags": [
+ "Getting Started with C++",
+ "Robot and Motor",
+ "XboxController",
+ "Complete List"
+ ],
+ "foldername": "ArcadeDriveXboxController",
+ "gradlebase": "cpp",
+ "commandversion": 1
+ },
+ {
+ "name": "Tank Drive Xbox Controller",
+ "description": "An example program which demonstrates the use of Tank Drive with the DifferentialDrive class and an Xbox Controller.",
+ "tags": [
+ "Getting Started with C++",
+ "Robot and Motor",
+ "XboxController",
+ "Complete List"
+ ],
+ "foldername": "TankDriveXboxController",
+ "gradlebase": "cpp",
+ "commandversion": 1
+ },
+ {
+ "name": "Duty Cycle Encoder",
+ "description": "Demonstrates the use of the Duty Cycle Encoder class",
+ "tags": [
+ "Getting Started with C++"
+ ],
+ "foldername": "DutyCycleEncoder",
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "Duty Cycle Input",
+ "description": "Demonstrates the use of the Duty Cycle class",
+ "tags": [
+ "Getting Started with C++"
+ ],
+ "foldername": "DutyCycleInput",
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "Addressable LED",
+ "description": "Demonstrates the use of the Addressable LED class",
+ "tags": [
+ "Getting Started with C++"
+ ],
+ "foldername": "AddressableLED",
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "DMA",
+ "description": "Demonstrates the use of the DMA class",
+ "tags": [
+ "Advanced C++"
+ ],
+ "foldername": "DMA",
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "MecanumControllerCommand",
+ "description": "An example command-based robot demonstrating the use of a MecanumControllerCommand to follow a pregenerated trajectory.",
+ "tags": [
+ "MecanumControllerCommand",
+ "Mecanum",
+ "PID",
+ "Trajectory",
+ "Path following"
+ ],
+ "foldername": "MecanumControllerCommand",
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "SwerveControllerCommand",
+ "description": "An example command-based robot demonstrating the use of a SwerveControllerCommand to follow a pregenerated trajectory.",
+ "tags": [
+ "SwerveControllerCommand",
+ "Swerve",
+ "PID",
+ "Trajectory",
+ "Path following"
+ ],
+ "foldername": "SwerveControllerCommand",
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "ArmBot",
+ "description": "An example command-based robot demonstrating the use of a ProfiledPIDSubsystem to control an arm.",
+ "tags": [
+ "ArmBot",
+ "PID",
+ "Motion Profile"
+ ],
+ "foldername": "ArmBot",
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "ArmBotOffboard",
+ "description": "An example command-based robot demonstrating the use of a TrapezoidProfileSubsystem to control an arm with an offboard PID.",
+ "tags": [
+ "ArmBotOffboard",
+ "PID",
+ "Motion Profile"
+ ],
+ "foldername": "ArmBotOffboard",
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "DriveDistanceOffboard",
+ "description": "An example command-based robot demonstrating the use of a TrapezoidProfileCommand to drive a robot a set distance with offboard PID on the drive.",
+ "tags": [
+ "DriveDistance",
+ "PID",
+ "Motion Profile"
+ ],
+ "foldername": "DriveDistanceOffboard",
+ "gradlebase": "cpp",
+ "commandversion": 2
}
]
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/iterative/cpp/Robot.cpp
deleted file mode 100644
index 942fc9a..0000000
--- a/wpilibcExamples/src/main/cpp/templates/iterative/cpp/Robot.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#include "Robot.h"
-
-#include <iostream>
-
-#include <frc/smartdashboard/SmartDashboard.h>
-
-void Robot::RobotInit() {
- m_chooser.SetDefaultOption(kAutoNameDefault, kAutoNameDefault);
- m_chooser.AddOption(kAutoNameCustom, kAutoNameCustom);
- frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
-}
-
-/**
- * This function is called every robot packet, no matter the mode. Use
- * this for items like diagnostics that you want ran during disabled,
- * autonomous, teleoperated and test.
- *
- * <p> This runs after the mode specific periodic functions, but before
- * LiveWindow and SmartDashboard integrated updating.
- */
-void Robot::RobotPeriodic() {}
-
-/**
- * This autonomous (along with the chooser code above) shows how to select
- * between different autonomous modes using the dashboard. The sendable chooser
- * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
- * remove all of the chooser code and uncomment the GetString line to get the
- * auto name from the text box below the Gyro.
- *
- * You can add additional auto modes by adding additional comparisons to the
- * if-else structure below with additional strings. If using the SendableChooser
- * make sure to add them to the chooser code above as well.
- */
-void Robot::AutonomousInit() {
- m_autoSelected = m_chooser.GetSelected();
- // m_autoSelected = SmartDashboard::GetString(
- // "Auto Selector", kAutoNameDefault);
- std::cout << "Auto selected: " << m_autoSelected << std::endl;
-
- if (m_autoSelected == kAutoNameCustom) {
- // Custom Auto goes here
- } else {
- // Default Auto goes here
- }
-}
-
-void Robot::AutonomousPeriodic() {
- if (m_autoSelected == kAutoNameCustom) {
- // Custom Auto goes here
- } else {
- // Default Auto goes here
- }
-}
-
-void Robot::TeleopInit() {}
-
-void Robot::TeleopPeriodic() {}
-
-void Robot::TestPeriodic() {}
-
-#ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
-#endif
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/OI.cpp
similarity index 64%
copy from wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
copy to wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/OI.cpp
index 8bd62ea..2a9ef50 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/OI.cpp
@@ -5,12 +5,8 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include "frc2/command/PrintCommand.h"
+#include "OI.h"
-using namespace frc2;
-
-PrintCommand::PrintCommand(const wpi::Twine& message)
- : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
+OI::OI() {
+ // Process operator interface input here.
}
-
-bool PrintCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/Robot.cpp
new file mode 100644
index 0000000..b2a8884
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/Robot.cpp
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/commands/Scheduler.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+ExampleSubsystem Robot::m_subsystem;
+OI Robot::m_oi;
+
+void Robot::RobotInit() {
+ m_chooser.SetDefaultOption("Default Auto", &m_defaultAuto);
+ m_chooser.AddOption("My Auto", &m_myAuto);
+ frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want ran during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() {}
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+
+/**
+ * This autonomous (along with the chooser code above) shows how to select
+ * between different autonomous modes using the dashboard. The sendable chooser
+ * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
+ * remove all of the chooser code and uncomment the GetString code to get the
+ * auto name from the text box below the Gyro.
+ *
+ * You can add additional auto modes by adding additional commands to the
+ * chooser code above (like the commented example) or additional comparisons to
+ * the if-else structure below with additional strings & commands.
+ */
+void Robot::AutonomousInit() {
+ // std::string autoSelected = frc::SmartDashboard::GetString(
+ // "Auto Selector", "Default");
+ // if (autoSelected == "My Auto") {
+ // m_autonomousCommand = &m_myAuto;
+ // } else {
+ // m_autonomousCommand = &m_defaultAuto;
+ // }
+
+ m_autonomousCommand = m_chooser.GetSelected();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Start();
+ }
+}
+
+void Robot::AutonomousPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+void Robot::TeleopPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/ExampleCommand.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/ExampleCommand.cpp
new file mode 100644
index 0000000..a5596ef
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/ExampleCommand.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/ExampleCommand.h"
+
+#include "Robot.h"
+
+ExampleCommand::ExampleCommand() {
+ // Use Requires() here to declare subsystem dependencies
+ Requires(&Robot::m_subsystem);
+}
+
+// Called just before this Command runs the first time
+void ExampleCommand::Initialize() {}
+
+// Called repeatedly when this Command is scheduled to run
+void ExampleCommand::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool ExampleCommand::IsFinished() { return false; }
+
+// Called once after isFinished returns true
+void ExampleCommand::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void ExampleCommand::Interrupted() {}
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/MyAutoCommand.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/MyAutoCommand.cpp
new file mode 100644
index 0000000..1389447
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/MyAutoCommand.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/MyAutoCommand.h"
+
+#include "Robot.h"
+
+MyAutoCommand::MyAutoCommand() {
+ // Use Requires() here to declare subsystem dependencies
+ Requires(&Robot::m_subsystem);
+}
+
+// Called just before this Command runs the first time
+void MyAutoCommand::Initialize() {}
+
+// Called repeatedly when this Command is scheduled to run
+void MyAutoCommand::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool MyAutoCommand::IsFinished() { return false; }
+
+// Called once after isFinished returns true
+void MyAutoCommand::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void MyAutoCommand::Interrupted() {}
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/subsystems/ExampleSubsystem.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/subsystems/ExampleSubsystem.cpp
new file mode 100644
index 0000000..ed60550
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/subsystems/ExampleSubsystem.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/ExampleSubsystem.h"
+
+#include "RobotMap.h"
+
+ExampleSubsystem::ExampleSubsystem() : frc::Subsystem("ExampleSubsystem") {}
+
+void ExampleSubsystem::InitDefaultCommand() {
+ // Set the default command for a subsystem here.
+ // SetDefaultCommand(new MySpecialCommand());
+}
+
+// Put methods for controlling this subsystem
+// here. Call these from Commands.
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/OI.h
similarity index 64%
copy from wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
copy to wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/OI.h
index 8bd62ea..77007bb 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/OI.h
@@ -5,12 +5,9 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include "frc2/command/PrintCommand.h"
+#pragma once
-using namespace frc2;
-
-PrintCommand::PrintCommand(const wpi::Twine& message)
- : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
-}
-
-bool PrintCommand::RunsWhenDisabled() const { return true; }
+class OI {
+ public:
+ OI();
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/Robot.h
new file mode 100644
index 0000000..94c2314
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/Robot.h
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc/commands/Command.h>
+#include <frc/smartdashboard/SendableChooser.h>
+
+#include "OI.h"
+#include "commands/ExampleCommand.h"
+#include "commands/MyAutoCommand.h"
+#include "subsystems/ExampleSubsystem.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+ static ExampleSubsystem m_subsystem;
+ static OI m_oi;
+
+ void RobotInit() override;
+ void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
+ void AutonomousInit() override;
+ void AutonomousPeriodic() override;
+ void TeleopInit() override;
+ void TeleopPeriodic() override;
+ void TestPeriodic() override;
+
+ private:
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc::Command* m_autonomousCommand = nullptr;
+ ExampleCommand m_defaultAuto;
+ MyAutoCommand m_myAuto;
+ frc::SendableChooser<frc::Command*> m_chooser;
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/RobotMap.h b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/RobotMap.h
new file mode 100644
index 0000000..875d3aa
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/RobotMap.h
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+/**
+ * The RobotMap is a mapping from the ports sensors and actuators are wired into
+ * to a variable name. This provides flexibility changing wiring, makes checking
+ * the wiring easier and significantly reduces the number of magic numbers
+ * floating around.
+ */
+
+// For example to map the left and right motors, you could define the
+// following variables to use with your drivetrain subsystem.
+// constexpr int kLeftMotor = 1;
+// constexpr int kRightMotor = 2;
+
+// If you are using multiple modules, make sure to define both the port
+// number and the module. For example you with a rangefinder:
+// constexpr int kRangeFinderPort = 1;
+// constexpr int kRangeFinderModule = 1;
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/ExampleCommand.h
similarity index 64%
copy from wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
copy to wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/ExampleCommand.h
index 8bd62ea..558fced 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/ExampleCommand.h
@@ -5,12 +5,16 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include "frc2/command/PrintCommand.h"
+#pragma once
-using namespace frc2;
+#include <frc/commands/Command.h>
-PrintCommand::PrintCommand(const wpi::Twine& message)
- : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
-}
-
-bool PrintCommand::RunsWhenDisabled() const { return true; }
+class ExampleCommand : public frc::Command {
+ public:
+ ExampleCommand();
+ void Initialize() override;
+ void Execute() override;
+ bool IsFinished() override;
+ void End() override;
+ void Interrupted() override;
+};
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/MyAutoCommand.h
similarity index 64%
copy from wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
copy to wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/MyAutoCommand.h
index 8bd62ea..f1892a7 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/MyAutoCommand.h
@@ -5,12 +5,16 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include "frc2/command/PrintCommand.h"
+#pragma once
-using namespace frc2;
+#include <frc/commands/Command.h>
-PrintCommand::PrintCommand(const wpi::Twine& message)
- : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
-}
-
-bool PrintCommand::RunsWhenDisabled() const { return true; }
+class MyAutoCommand : public frc::Command {
+ public:
+ MyAutoCommand();
+ void Initialize() override;
+ void Execute() override;
+ bool IsFinished() override;
+ void End() override;
+ void Interrupted() override;
+};
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/subsystems/ExampleSubsystem.h
similarity index 61%
copy from wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
copy to wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/subsystems/ExampleSubsystem.h
index 8bd62ea..0f7d470 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/subsystems/ExampleSubsystem.h
@@ -5,12 +5,16 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include "frc2/command/PrintCommand.h"
+#pragma once
-using namespace frc2;
+#include <frc/commands/Subsystem.h>
-PrintCommand::PrintCommand(const wpi::Twine& message)
- : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
-}
+class ExampleSubsystem : public frc::Subsystem {
+ public:
+ ExampleSubsystem();
+ void InitDefaultCommand() override;
-bool PrintCommand::RunsWhenDisabled() const { return true; }
+ private:
+ // It's desirable that everything possible under private except
+ // for methods that implement subsystem capabilities
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
index 8495b63..de8fc3e 100644
--- a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
@@ -7,11 +7,10 @@
#include "Robot.h"
-#include <hal/DriverStation.h>
-
#include <frc/DriverStation.h>
#include <frc/livewindow/LiveWindow.h>
#include <frc/shuffleboard/Shuffleboard.h>
+#include <hal/DriverStation.h>
#include <networktables/NetworkTable.h>
void Robot::RobotInit() {}
@@ -32,7 +31,7 @@
// Tell the DS that the robot is ready to be enabled
HAL_ObserveUserProgramStarting();
- while (true) {
+ while (!m_exit) {
if (IsDisabled()) {
m_ds.InDisabled(true);
Disabled();
@@ -61,6 +60,8 @@
}
}
+void Robot::EndCompetition() { m_exit = true; }
+
#ifndef RUNNING_FRC_TESTS
int main() { return frc::StartRobot<Robot>(); }
#endif
diff --git a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h
index 4efc087..0057778 100644
--- a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h
@@ -7,6 +7,8 @@
#pragma once
+#include <atomic>
+
#include <frc/RobotBase.h>
class Robot : public frc::RobotBase {
@@ -18,4 +20,8 @@
void Test();
void StartCompetition() override;
+ void EndCompetition() override;
+
+ private:
+ std::atomic<bool> m_exit{false};
};
diff --git a/wpilibcExamples/src/main/cpp/templates/templates.json b/wpilibcExamples/src/main/cpp/templates/templates.json
index 71798c9..c31d41c 100644
--- a/wpilibcExamples/src/main/cpp/templates/templates.json
+++ b/wpilibcExamples/src/main/cpp/templates/templates.json
@@ -1,21 +1,13 @@
[
{
- "name": "Iterative Robot",
- "description": "Iterative style",
- "tags": [
- "Iterative"
- ],
- "foldername": "iterative",
- "gradlebase": "cpp"
- },
- {
"name": "Timed Robot",
"description": "Timed style",
"tags": [
"Timed"
],
"foldername": "timed",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Timed Skeleton (Advanced)",
@@ -24,7 +16,8 @@
"Timed", "Skeleton"
],
"foldername": "timedskeleton",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "RobotBase Skeleton (Advanced)",
@@ -33,7 +26,8 @@
"RobotBase", "Skeleton"
],
"foldername": "robotbaseskeleton",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Command Robot",
@@ -42,6 +36,17 @@
"Command"
],
"foldername": "commandbased",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "Old Command Robot",
+ "description": "Old-Command style (deprecated)",
+ "tags": [
+ "Command"
+ ],
+ "foldername": "oldcommandbased",
+ "gradlebase": "cpp",
+ "commandversion": 1
}
]
diff --git a/wpilibcIntegrationTests/build.gradle b/wpilibcIntegrationTests/build.gradle
index 8a3797b..ed6d58e 100644
--- a/wpilibcIntegrationTests/build.gradle
+++ b/wpilibcIntegrationTests/build.gradle
@@ -19,14 +19,6 @@
apply from: "${rootDir}/shared/googletest.gradle"
-ext {
- chipObjectComponents = ['wpilibcIntegrationTests']
- netCommComponents = ['wpilibcIntegrationTests']
- useNiJava = false
-}
-
-apply from: "${rootDir}/shared/nilibraries.gradle"
-
model {
components {
wpilibcIntegrationTests(NativeExecutableSpec) {
@@ -52,6 +44,7 @@
cppCompiler.args "-Wno-unused-variable"
cppCompiler.args "-Wno-error=deprecated-declarations"
}
+ lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
lib project: ':cscore', library: 'cscore', linkage: 'shared'
@@ -61,6 +54,9 @@
project(':hal').addHalJniDependency(binary)
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+ if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
} else {
binary.sources {
simCpp(CppSourceSet) {
diff --git a/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp b/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp
index 74242df..e368fc2 100644
--- a/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp
+++ b/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp
@@ -5,6 +5,8 @@
/* the project. */
/*----------------------------------------------------------------------------*/
+#include <algorithm>
+
#include "TestBench.h"
#include "frc/Encoder.h"
#include "frc/Jaguar.h"
@@ -46,7 +48,7 @@
protected:
SpeedController* m_speedController;
Encoder* m_encoder;
- LinearFilter* m_filter;
+ LinearFilter<double>* m_filter;
void SetUp() override {
switch (GetParam()) {
@@ -68,7 +70,8 @@
TestBench::kTalonEncoderChannelB);
break;
}
- m_filter = new LinearFilter(LinearFilter::MovingAverage(50));
+ m_filter =
+ new LinearFilter<double>(LinearFilter<double>::MovingAverage(50));
}
void TearDown() override {
@@ -146,7 +149,8 @@
/* 10 seconds should be plenty time to get to the reference */
frc::Notifier pidRunner{[this, &pidController] {
- m_speedController->Set(pidController.Calculate(m_encoder->GetDistance()));
+ auto speed = pidController.Calculate(m_encoder->GetDistance());
+ m_speedController->Set(std::clamp(speed, -0.2, 0.2));
}};
pidRunner.StartPeriodic(pidController.GetPeriod());
Wait(10.0);
@@ -171,9 +175,9 @@
/* 10 seconds should be plenty time to get to the reference */
frc::Notifier pidRunner{[this, &pidController] {
- m_speedController->Set(
- pidController.Calculate(m_filter->Calculate(m_encoder->GetRate())) +
- 8e-5);
+ auto speed =
+ pidController.Calculate(m_filter->Calculate(m_encoder->GetRate()));
+ m_speedController->Set(std::clamp(speed, -0.3, 0.3));
}};
pidRunner.StartPeriodic(pidController.GetPeriod());
Wait(10.0);
diff --git a/wpilibj/CMakeLists.txt b/wpilibj/CMakeLists.txt
index ad554c7..7874f7f 100644
--- a/wpilibj/CMakeLists.txt
+++ b/wpilibj/CMakeLists.txt
@@ -15,9 +15,10 @@
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/*.jar")
+ 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} ${OPENCV_JAR_FILE} cscore_jar cameraserver_jar wpiutil_jar OUTPUT_NAME wpilibj)
+ 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}")
diff --git a/wpilibj/build.gradle b/wpilibj/build.gradle
index 83a6e55..cf6c4e5 100644
--- a/wpilibj/build.gradle
+++ b/wpilibj/build.gradle
@@ -60,19 +60,19 @@
}
dependencies {
- compile project(':hal')
- compile project(':wpiutil')
- compile project(':ntcore')
- compile project(':cscore')
- compile project(':cameraserver')
- testCompile 'com.google.guava:guava:19.0'
- testCompile 'org.mockito:mockito-core:2.27.0'
- devCompile project(':hal')
- devCompile project(':wpiutil')
- devCompile project(':ntcore')
- devCompile project(':cscore')
- devCompile project(':cameraserver')
- devCompile sourceSets.main.output
+ 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'
@@ -89,14 +89,6 @@
useCpp = true
}
-ext {
- chipObjectComponents = ['wpilibjDev']
- netCommComponents = ['wpilibjDev']
- useNiJava = true
-}
-
-apply from: "${rootDir}/shared/nilibraries.gradle"
-
apply from: "${rootDir}/shared/opencv.gradle"
model {
@@ -108,13 +100,6 @@
source {
srcDirs 'src/dev/native/cpp'
include '**/*.cpp'
- 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: 'wpiutil', linkage: 'shared'
- lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
}
exportedHeaders {
srcDirs 'src/dev/native/include'
@@ -122,8 +107,18 @@
}
}
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')
+ }
}
}
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
index 06daf85..54583ea 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
@@ -129,7 +129,7 @@
transferBuffer.put(2, (byte) (kPowerCtl_Measure | kPowerCtl_UltraLowNoise));
m_spi.write(transferBuffer, 3);
- HAL.report(tResourceType.kResourceType_ADXL362, port.value);
+ HAL.report(tResourceType.kResourceType_ADXL362, port.value + 1);
SendableRegistry.addLW(this, "ADXL362", port.value);
}
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
index 7658072..549a91a 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
@@ -95,7 +95,7 @@
calibrate();
}
- HAL.report(tResourceType.kResourceType_ADXRS450, port.value);
+ HAL.report(tResourceType.kResourceType_ADXRS450, port.value + 1);
SendableRegistry.addLW(this, "ADXRS450_Gyro", port.value);
}
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..8f9c1b4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+/**
+ * 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;
+ }
+ }
+
+ /**
+ * 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
index 4a4c675..41c5b38 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
@@ -31,7 +31,7 @@
*/
private void initAccelerometer() {
HAL.report(tResourceType.kResourceType_Accelerometer,
- m_analogChannel.getChannel());
+ m_analogChannel.getChannel() + 1);
SendableRegistry.addLW(this, "Accelerometer", m_analogChannel.getChannel());
}
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
index f7e3f2f..c688b55 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
@@ -41,7 +41,7 @@
AnalogGyroJNI.setupAnalogGyro(m_gyroHandle);
- HAL.report(tResourceType.kResourceType_Gyro, m_analog.getChannel());
+ HAL.report(tResourceType.kResourceType_Gyro, m_analog.getChannel() + 1);
SendableRegistry.addLW(this, "AnalogGyro", m_analog.getChannel());
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
index 0e1260e..308be6b 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
@@ -49,7 +49,7 @@
final int portHandle = HAL.getPort((byte) channel);
m_port = AnalogJNI.initializeAnalogInputPort(portHandle);
- HAL.report(tResourceType.kResourceType_AnalogChannel, channel);
+ HAL.report(tResourceType.kResourceType_AnalogChannel, channel + 1);
SendableRegistry.addLW(this, "AnalogInput", 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
index 902bec6..ae03463 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java
@@ -33,7 +33,7 @@
final int portHandle = HAL.getPort((byte) channel);
m_port = AnalogJNI.initializeAnalogOutputPort(portHandle);
- HAL.report(tResourceType.kResourceType_AnalogOutput, channel);
+ HAL.report(tResourceType.kResourceType_AnalogOutput, channel + 1);
SendableRegistry.addLW(this, "AnalogOutput", channel);
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
index 5bff3ea..f7f98bd 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
@@ -7,9 +7,6 @@
package edu.wpi.first.wpilibj;
-import java.nio.ByteBuffer;
-import java.nio.ByteOrder;
-
import edu.wpi.first.hal.AnalogJNI;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
@@ -42,8 +39,8 @@
* Where the analog trigger is attached.
*/
protected int m_port;
- protected int m_index;
protected AnalogInput m_analogInput;
+ protected DutyCycle m_dutyCycle;
protected boolean m_ownsAnalog;
/**
@@ -65,15 +62,29 @@
*/
public AnalogTrigger(AnalogInput channel) {
m_analogInput = channel;
- ByteBuffer index = ByteBuffer.allocateDirect(4);
- index.order(ByteOrder.LITTLE_ENDIAN);
- m_port =
- AnalogJNI.initializeAnalogTrigger(channel.m_port, index.asIntBuffer());
- m_index = index.asIntBuffer().get(0);
+ m_port = AnalogJNI.initializeAnalogTrigger(channel.m_port);
- HAL.report(tResourceType.kResourceType_AnalogTrigger, channel.getChannel());
- SendableRegistry.addLW(this, "AnalogTrigger", channel.getChannel());
+ 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
@@ -102,6 +113,20 @@
/**
* 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
@@ -141,8 +166,8 @@
*
* @return The index of the analog trigger.
*/
- public int getIndex() {
- return m_index;
+ public final int getIndex() {
+ return AnalogJNI.getAnalogTriggerFPGAIndex(m_port);
}
/**
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
index 049beb3..7635196 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
@@ -74,7 +74,7 @@
m_trigger = trigger;
m_outputType = outputType;
HAL.report(tResourceType.kResourceType_AnalogTriggerOutput,
- trigger.getIndex(), outputType.value);
+ trigger.getIndex() + 1, outputType.value + 1);
}
/**
@@ -98,7 +98,7 @@
@Override
public int getChannel() {
- return m_trigger.m_index;
+ return m_trigger.getIndex();
}
@Override
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
index 759056f..7b188e5 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
@@ -40,7 +40,7 @@
*/
public CAN(int deviceId) {
m_handle = CANAPIJNI.initializeCAN(kTeamManufacturer, deviceId, kTeamDeviceType);
- HAL.report(tResourceType.kResourceType_CAN, deviceId);
+ HAL.report(tResourceType.kResourceType_CAN, deviceId + 1);
}
/**
@@ -54,7 +54,7 @@
*/
public CAN(int deviceId, int deviceManufacturer, int deviceType) {
m_handle = CANAPIJNI.initializeCAN(deviceManufacturer, deviceId, deviceType);
- HAL.report(tResourceType.kResourceType_CAN, deviceId);
+ HAL.report(tResourceType.kResourceType_CAN, deviceId + 1);
}
/**
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
index cddf2ef..9d8a8dd 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
@@ -39,7 +39,7 @@
m_compressorHandle = CompressorJNI.initializeCompressor((byte) module);
- HAL.report(tResourceType.kResourceType_Compressor, module);
+ HAL.report(tResourceType.kResourceType_Compressor, module + 1);
SendableRegistry.addLW(this, "Compressor", module);
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
index bdb81fd..0a26810 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
@@ -84,9 +84,9 @@
m_upSource = null;
m_downSource = null;
- setMaxPeriod(.5);
+ setMaxPeriod(0.5);
- HAL.report(tResourceType.kResourceType_Counter, m_index, mode.value);
+ HAL.report(tResourceType.kResourceType_Counter, m_index + 1, mode.value + 1);
SendableRegistry.addLW(this, "Counter", m_index);
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMC60.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMC60.java
index f414773..0feeebb 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMC60.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMC60.java
@@ -13,33 +13,37 @@
/**
* 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.
*
- * <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>- 2.004ms = full "forward" - 1.52ms = the "high end" of the deadband range - 1.50ms =
- * center of the deadband range (off) - 1.48ms = the "low end" of the deadband range - .997ms =
- * full "reverse"
- *
* @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, .997);
+ setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
- HAL.report(tResourceType.kResourceType_DigilentDMC60, getChannel());
+ 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
index 777f2f9..10155cc 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java
@@ -35,7 +35,7 @@
m_channelIndex = index;
m_filterAllocated[index] = true;
HAL.report(tResourceType.kResourceType_DigitalGlitchFilter,
- m_channelIndex, 0);
+ m_channelIndex + 1, 0);
SendableRegistry.addLW(this, "DigitalGlitchFilter", index);
}
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java
index 433e3fd..7cbd1ed 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java
@@ -35,7 +35,7 @@
m_handle = DIOJNI.initializeDIOPort(HAL.getPort((byte) channel), true);
- HAL.report(tResourceType.kResourceType_DigitalInput, channel);
+ HAL.report(tResourceType.kResourceType_DigitalInput, channel + 1);
SendableRegistry.addLW(this, "DigitalInput", channel);
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java
index ede25f2..ec1a44c 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java
@@ -18,7 +18,7 @@
* 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 implements Sendable, AutoCloseable {
+public class DigitalOutput extends DigitalSource implements Sendable, AutoCloseable {
private static final int invalidPwmGenerator = 0;
private int m_pwmGenerator = invalidPwmGenerator;
@@ -38,12 +38,13 @@
m_handle = DIOJNI.initializeDIOPort(HAL.getPort((byte) channel), false);
- HAL.report(tResourceType.kResourceType_DigitalOutput, channel);
+ 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) {
@@ -76,6 +77,7 @@
*
* @return The GPIO channel number.
*/
+ @Override
public int getChannel() {
return m_channel;
}
@@ -177,4 +179,34 @@
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/DoubleSolenoid.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
index 1002945..d612d24 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
@@ -77,10 +77,10 @@
m_forwardMask = (byte) (1 << forwardChannel);
m_reverseMask = (byte) (1 << reverseChannel);
- HAL.report(tResourceType.kResourceType_Solenoid, forwardChannel,
- m_moduleNumber);
- HAL.report(tResourceType.kResourceType_Solenoid, reverseChannel,
- m_moduleNumber);
+ 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);
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
index 5955201..997fe2b 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
@@ -1009,6 +1009,16 @@
}
/**
+ * 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.
*/
@@ -1061,11 +1071,7 @@
m_cacheDataMutex.unlock();
}
- m_waitForDataMutex.lock();
- m_waitForDataCount++;
- m_waitForDataCond.signalAll();
- m_waitForDataMutex.unlock();
-
+ wakeupWaitForData();
sendMatchData();
}
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
index a6ea302..125f769 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java
@@ -80,7 +80,7 @@
m_pidSource = PIDSourceType.kDisplacement;
int fpgaIndex = getFPGAIndex();
- HAL.report(tResourceType.kResourceType_Encoder, fpgaIndex, type.value);
+ HAL.report(tResourceType.kResourceType_Encoder, fpgaIndex + 1, type.value + 1);
SendableRegistry.addLW(this, "Encoder", fpgaIndex);
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java
index 80b5804..41f9d09 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java
@@ -56,9 +56,9 @@
super(channel);
enableDirectionSensing(directionSensitive);
if (directionSensitive) {
- HAL.report(tResourceType.kResourceType_GearTooth, channel, 0, "D");
+ HAL.report(tResourceType.kResourceType_GearTooth, channel + 1, 0, "D");
} else {
- HAL.report(tResourceType.kResourceType_GearTooth, channel, 0);
+ HAL.report(tResourceType.kResourceType_GearTooth, channel + 1, 0);
}
SendableRegistry.setName(this, "GearTooth", channel);
}
@@ -75,9 +75,9 @@
super(source);
enableDirectionSensing(directionSensitive);
if (directionSensitive) {
- HAL.report(tResourceType.kResourceType_GearTooth, source.getChannel(), 0, "D");
+ HAL.report(tResourceType.kResourceType_GearTooth, source.getChannel() + 1, 0, "D");
} else {
- HAL.report(tResourceType.kResourceType_GearTooth, source.getChannel(), 0);
+ HAL.report(tResourceType.kResourceType_GearTooth, source.getChannel() + 1, 0);
}
SendableRegistry.setName(this, "GearTooth", source.getChannel());
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java
index 01fa26a..f1baefb 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java
@@ -25,6 +25,7 @@
@Deprecated
public class IterativeRobot extends IterativeRobotBase {
private static final double kPacketPeriod = 0.02;
+ private volatile boolean m_exit;
/**
* Create a new IterativeRobot.
@@ -49,8 +50,20 @@
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/Jaguar.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java
index b863f2e..8ce7b7d 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java
@@ -13,6 +13,20 @@
/**
* 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 {
/**
@@ -24,20 +38,12 @@
public Jaguar(final int channel) {
super(channel);
- /*
- * Input profile defined by Luminary Micro.
- *
- * Full reverse ranges from 0.671325ms to 0.6972211ms Proportional reverse
- * ranges from 0.6972211ms to 1.4482078ms Neutral ranges from 1.4482078ms to
- * 1.5517922ms Proportional forward ranges from 1.5517922ms to 2.3027789ms
- * Full forward ranges from 2.3027789ms to 2.328675ms
- */
- setBounds(2.31, 1.55, 1.507, 1.454, .697);
+ setBounds(2.31, 1.55, 1.507, 1.454, 0.697);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
- HAL.report(tResourceType.kResourceType_Jaguar, getChannel());
+ 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
index 06a4cdc..60a60bb 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java
@@ -97,7 +97,7 @@
m_axes[Axis.kTwist.value] = kDefaultTwistChannel;
m_axes[Axis.kThrottle.value] = kDefaultThrottleChannel;
- HAL.report(tResourceType.kResourceType_Joystick, port);
+ HAL.report(tResourceType.kResourceType_Joystick, port + 1);
}
/**
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/NidecBrushless.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java
index 4bfee48..9d1011b 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java
@@ -44,7 +44,7 @@
m_pwm = new PWM(pwmChannel);
SendableRegistry.addChild(this, m_pwm);
- HAL.report(tResourceType.kResourceType_NidecBrushless, pwmChannel);
+ HAL.report(tResourceType.kResourceType_NidecBrushless, pwmChannel + 1);
SendableRegistry.addLW(this, "Nidec Brushless", pwmChannel);
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
index e1d6bb6..806bfa9 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
@@ -139,6 +139,16 @@
}
/**
+ * 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
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java
index 0647291..8febd2c 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java
@@ -46,7 +46,9 @@
}
private final int m_channel;
- private int m_handle;
+
+ // Package private to use from AddressableLED
+ int m_handle;
/**
* Allocate a PWM given a channel.
@@ -63,7 +65,7 @@
PWMJNI.setPWMEliminateDeadband(m_handle, false);
- HAL.report(tResourceType.kResourceType_PWM, channel);
+ HAL.report(tResourceType.kResourceType_PWM, channel + 1);
SendableRegistry.addLW(this, "PWM", channel);
setSafetyEnabled(false);
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSparkMax.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSparkMax.java
index 29b28f2..0f97ea5 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSparkMax.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSparkMax.java
@@ -12,31 +12,36 @@
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
/**
- * REV Robotics SparkMax Speed Controller.
+ * 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.
*
- * <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>- 2.003ms = full "forward" - 1.55ms = the "high end" of the deadband range - 1.50ms =
- * center of the deadband range (off) - 1.46ms = the "low end" of the deadband range - .999ms =
- * full "reverse"
*/
public PWMSparkMax(final int channel) {
super(channel);
- setBounds(2.003, 1.55, 1.50, 1.46, .999);
+ setBounds(2.003, 1.55, 1.50, 1.46, 0.999);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
- HAL.report(tResourceType.kResourceType_RevSparkMaxPWM, getChannel());
+ HAL.report(tResourceType.kResourceType_RevSparkMaxPWM, getChannel() + 1);
SendableRegistry.setName(this, "PWMSparkMax", 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
index 691210e..1859e7f 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java
@@ -13,34 +13,37 @@
/**
* 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 PWMTalonSRX connected via PWM.
+ * Constructor for a TalonSRX connected via PWM.
*
- * <p>Note that the PWMTalonSRX 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>- 2.0004ms = full "forward" - 1.52ms = the "high end" of the deadband range - 1.50ms =
- * center
- * of the deadband range (off) - 1.48ms = the "low end" of the deadband range - .997ms = full
- * "reverse"
- *
- * @param channel The PWM channel that the PWMTalonSRX is attached to. 0-9 are on-board, 10-19 are
+ * @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, .997);
+ setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
- HAL.report(tResourceType.kResourceType_PWMTalonSRX, getChannel());
+ HAL.report(tResourceType.kResourceType_PWMTalonSRX, getChannel() + 1);
SendableRegistry.setName(this, "PWMTalonSRX", 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
index 0607630..b51686a 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVictorSPX.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVictorSPX.java
@@ -13,21 +13,24 @@
/**
* 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 PWMVictorSPX connected via PWM.
- *
- * <p>Note that the PWMVictorSPX 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 VictorSPX User
- * Manual available from CTRE.
- *
- * <p>- 2.0004ms = full "forward" - 1.52ms = the "high end" of the deadband range - 1.50ms =
- * center
- * of the deadband range (off) - 1.48ms = the "low end" of the deadband range - .997ms = full
- * "reverse"
+ * 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
@@ -35,12 +38,12 @@
public PWMVictorSPX(final int channel) {
super(channel);
- setBounds(2.004, 1.52, 1.50, 1.48, .997);
+ setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
- HAL.report(tResourceType.kResourceType_PWMVictorSPX, getChannel());
+ 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
index 8030daa..e30be1b 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java
@@ -29,7 +29,7 @@
SensorUtil.checkPDPModule(module);
m_handle = PDPJNI.initializePDP(module);
- HAL.report(tResourceType.kResourceType_PDP, module);
+ HAL.report(tResourceType.kResourceType_PDP, module + 1);
SendableRegistry.addLW(this, "PowerDistributionPanel", module);
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
index bea2491..fde4ef1 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
@@ -105,7 +105,7 @@
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);
+ HAL.report(tResourceType.kResourceType_Relay, m_channel + 1);
}
if (m_direction == Direction.kBoth || m_direction == Direction.kReverse) {
m_reverseHandle = RelayJNI.initializeRelayPort(portHandle, 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
index fa6263a..94ad884 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
@@ -12,6 +12,7 @@
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;
@@ -45,12 +46,12 @@
@Override
public void reportVideoServer(int id) {
- HAL.report(tResourceType.kResourceType_PCVideoServer, id);
+ HAL.report(tResourceType.kResourceType_PCVideoServer, id + 1);
}
@Override
public void reportUsbCamera(int id) {
- HAL.report(tResourceType.kResourceType_UsbCamera, id);
+ HAL.report(tResourceType.kResourceType_UsbCamera, id + 1);
}
@Override
@@ -60,7 +61,7 @@
@Override
public void reportAxisCamera(int id) {
- HAL.report(tResourceType.kResourceType_AxisCamera, id);
+ HAL.report(tResourceType.kResourceType_AxisCamera, id + 1);
}
@Override
@@ -189,6 +190,11 @@
*/
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);
@@ -204,6 +210,10 @@
}
}
+ private static final ReentrantLock m_runMutex = new ReentrantLock();
+ private static RobotBase m_robotCopy;
+ private static boolean m_suppressExitWarning;
+
/**
* Run the robot main loop.
*/
@@ -232,6 +242,10 @@
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");
@@ -265,19 +279,33 @@
throwable.getStackTrace());
errorOnExit = true;
} finally {
- // 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);
+ 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) {
@@ -299,6 +327,18 @@
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);
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
index 7a17273..104fb26 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
@@ -178,7 +178,7 @@
double value = Math.log(-curve);
double ratio = (value - m_sensitivity) / (value + m_sensitivity);
if (ratio == 0) {
- ratio = .0000000001;
+ ratio = 0.0000000001;
}
leftOutput = outputMagnitude / ratio;
rightOutput = outputMagnitude;
@@ -186,7 +186,7 @@
double value = Math.log(curve);
double ratio = (value - m_sensitivity) / (value + m_sensitivity);
if (ratio == 0) {
- ratio = .0000000001;
+ ratio = 0.0000000001;
}
leftOutput = outputMagnitude;
rightOutput = outputMagnitude / ratio;
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java
index cf81df6..b2e98b6 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java
@@ -13,28 +13,32 @@
/**
* 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.
- *
- * <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>- 2.05ms = full "forward" - 1.55ms = the "high end" of the deadband range - 1.50ms = center
- * of the deadband range (off) - 1.44ms = the "low end" of the deadband range - .94ms = full
- * "reverse"
*/
protected void initSD540() {
- setBounds(2.05, 1.55, 1.50, 1.44, .94);
+ setBounds(2.05, 1.55, 1.50, 1.44, 0.94);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
- HAL.report(tResourceType.kResourceType_MindsensorsSD540, getChannel());
+ HAL.report(tResourceType.kResourceType_MindsensorsSD540, getChannel() + 1);
SendableRegistry.setName(this, "SD540", getChannel());
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
index a76ebb1..97d9b27 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
@@ -47,7 +47,7 @@
SPIJNI.spiInitialize(m_port);
- HAL.report(tResourceType.kResourceType_SPI, port.value);
+ HAL.report(tResourceType.kResourceType_SPI, port.value + 1);
}
@Override
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
index 9cf0dcb..86bd941 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
@@ -120,7 +120,7 @@
disableTermination();
- HAL.report(tResourceType.kResourceType_SerialPort, 0);
+ HAL.report(tResourceType.kResourceType_SerialPort, port.value + 1);
}
/**
@@ -151,7 +151,7 @@
disableTermination();
- HAL.report(tResourceType.kResourceType_SerialPort, 0);
+ HAL.report(tResourceType.kResourceType_SerialPort, port.value + 1);
}
/**
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java
index dee7051..6b2eab6 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java
@@ -23,7 +23,7 @@
private static final double kMinServoAngle = 0.0;
protected static final double kDefaultMaxServoPWM = 2.4;
- protected static final double kDefaultMinServoPWM = .6;
+ protected static final double kDefaultMinServoPWM = 0.6;
/**
* Constructor.<br>
@@ -39,7 +39,7 @@
setBounds(kDefaultMaxServoPWM, 0, 0, 0, kDefaultMinServoPWM);
setPeriodMultiplier(PeriodMultiplier.k4X);
- HAL.report(tResourceType.kResourceType_Servo, getChannel());
+ HAL.report(tResourceType.kResourceType_Servo, getChannel() + 1);
SendableRegistry.setName(this, "Servo", getChannel());
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
index dc9b920..163415c 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
@@ -48,7 +48,7 @@
int portHandle = HAL.getPortWithModule((byte) m_moduleNumber, (byte) m_channel);
m_solenoidHandle = SolenoidJNI.initializeSolenoidPort(portHandle);
- HAL.report(tResourceType.kResourceType_Solenoid, m_channel, m_moduleNumber);
+ HAL.report(tResourceType.kResourceType_Solenoid, m_channel + 1, m_moduleNumber + 1);
SendableRegistry.addLW(this, "Solenoid", m_moduleNumber, m_channel);
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java
index ea2a8a7..c8bacbf 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java
@@ -13,28 +13,32 @@
/**
* 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.
- *
- * <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>- 2.003ms = full "forward" - 1.55ms = the "high end" of the deadband range - 1.50ms =
- * center of the deadband range (off) - 1.46ms = the "low end" of the deadband range - .999ms =
- * full "reverse"
*/
protected void initSpark() {
- setBounds(2.003, 1.55, 1.50, 1.46, .999);
+ setBounds(2.003, 1.55, 1.50, 1.46, 0.999);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
- HAL.report(tResourceType.kResourceType_RevSPARK, getChannel());
+ HAL.report(tResourceType.kResourceType_RevSPARK, getChannel() + 1);
SendableRegistry.setName(this, "Spark", getChannel());
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java
index b90d19f..4407ff0 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -19,6 +19,21 @@
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.
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java
index 861fca6..6f20869 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java
@@ -13,33 +13,37 @@
/**
* 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).
*
- * <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>- 2.037ms = full "forward" - 1.539ms = the "high end" of the deadband range - 1.513ms =
- * center of the deadband range (off) - 1.487ms = the "low end" of the deadband range - .989ms
- * = full "reverse"
- *
* @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, .989);
+ setBounds(2.037, 1.539, 1.513, 1.487, 0.989);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
- HAL.report(tResourceType.kResourceType_Talon, getChannel());
+ HAL.report(tResourceType.kResourceType_Talon, getChannel() + 1);
SendableRegistry.setName(this, "Talon", getChannel());
}
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
index 9575bf1..b6b6b25 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -43,6 +43,7 @@
*/
protected TimedRobot(double period) {
super(period);
+ NotifierJNI.setNotifierName(m_notifier, "TimedRobot");
HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Timed);
}
@@ -83,6 +84,14 @@
}
/**
+ * 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() {
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java
index fa9dc17..2d19240 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java
@@ -14,22 +14,26 @@
/**
* 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.
*
- * <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>- 2.027ms = full "forward" - 1.525ms = the "high end" of the deadband range - 1.507ms =
- * center of the deadband range (off) - 1.49ms = the "low end" of the deadband range - 1.026ms =
- * full "reverse"
- *
* @param channel The PWM channel that the Victor is attached to. 0-9 are
* on-board, 10-19 are on the MXP port
*/
@@ -41,7 +45,7 @@
setSpeed(0.0);
setZeroLatch();
- HAL.report(tResourceType.kResourceType_Victor, getChannel());
+ 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
index 019153e..22b3513 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java
@@ -13,33 +13,37 @@
/**
* 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.
*
- * <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>- 2.004ms = full "forward" - 1.52ms = the "high end" of the deadband range - 1.50ms =
- * center of the deadband range (off) - 1.48ms = the "low end" of the deadband range - .997ms =
- * full "reverse"
- *
* @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, .997);
+ setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
- HAL.report(tResourceType.kResourceType_VictorSP, getChannel());
+ HAL.report(tResourceType.kResourceType_VictorSP, getChannel() + 1);
SendableRegistry.setName(this, "VictorSP", getChannel());
}
}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
index 7f8dc63..fd6161a 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
@@ -50,7 +50,7 @@
public XboxController(final int port) {
super(port);
- HAL.report(tResourceType.kResourceType_XboxController, port);
+ HAL.report(tResourceType.kResourceType_XboxController, port + 1);
}
/**
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..355b182
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
@@ -0,0 +1,142 @@
+/*----------------------------------------------------------------------------*/
+/* 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 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 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
index c7bdb25..04e34b1 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
@@ -12,7 +12,7 @@
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.MathUtils;
+import edu.wpi.first.wpiutil.math.MathUtil;
/**
* Implements a PID control loop.
@@ -197,7 +197,7 @@
*/
public void setSetpoint(double setpoint) {
if (m_maximumInput > m_minimumInput) {
- m_setpoint = MathUtils.clamp(setpoint, m_minimumInput, m_maximumInput);
+ m_setpoint = MathUtil.clamp(setpoint, m_minimumInput, m_maximumInput);
} else {
m_setpoint = setpoint;
}
@@ -320,7 +320,7 @@
m_velocityError = (m_positionError - m_prevError) / m_period;
if (m_Ki != 0) {
- m_totalError = MathUtils.clamp(m_totalError + m_positionError * m_period,
+ m_totalError = MathUtil.clamp(m_totalError + m_positionError * m_period,
m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
}
@@ -378,7 +378,7 @@
// Clamp setpoint to new input
if (m_maximumInput > m_minimumInput) {
- m_setpoint = MathUtils.clamp(m_setpoint, m_minimumInput, m_maximumInput);
+ m_setpoint = MathUtil.clamp(m_setpoint, m_minimumInput, m_maximumInput);
}
}
}
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
index 77b7688..3ff9ac5 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
@@ -64,6 +64,15 @@
}
/**
+ * 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() {
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
index 1301629..cbfd3c0 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
@@ -17,7 +17,7 @@
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.MathUtils;
+import edu.wpi.first.wpiutil.math.MathUtil;
/**
* A class for driving differential drive/skid-steer drive platforms such as the Kit of Parts drive
@@ -31,12 +31,12 @@
* <p>Four motor drivetrain:
* <pre><code>
* public class Robot {
- * Spark m_frontLeft = new Spark(1);
- * Spark m_rearLeft = new Spark(2);
+ * SpeedController m_frontLeft = new PWMVictorSPX(1);
+ * SpeedController m_rearLeft = new PWMVictorSPX(2);
* SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_rearLeft);
*
- * Spark m_frontRight = new Spark(3);
- * Spark m_rearRight = new Spark(4);
+ * 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);
@@ -46,14 +46,14 @@
* <p>Six motor drivetrain:
* <pre><code>
* public class Robot {
- * Spark m_frontLeft = new Spark(1);
- * Spark m_midLeft = new Spark(2);
- * Spark m_rearLeft = new Spark(3);
+ * 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);
*
- * Spark m_frontRight = new Spark(4);
- * Spark m_midRight = new Spark(5);
- * Spark m_rearRight = new Spark(6);
+ * 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);
@@ -184,10 +184,10 @@
m_reported = true;
}
- xSpeed = MathUtils.clamp(xSpeed, -1.0, 1.0);
+ xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
xSpeed = applyDeadband(xSpeed, m_deadband);
- zRotation = MathUtils.clamp(zRotation, -1.0, 1.0);
+ zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
zRotation = applyDeadband(zRotation, m_deadband);
// Square the inputs (while preserving the sign) to increase fine control
@@ -222,9 +222,9 @@
}
}
- m_leftMotor.set(MathUtils.clamp(leftMotorOutput, -1.0, 1.0) * m_maxOutput);
+ m_leftMotor.set(MathUtil.clamp(leftMotorOutput, -1.0, 1.0) * m_maxOutput);
double maxOutput = m_maxOutput * m_rightSideInvertMultiplier;
- m_rightMotor.set(MathUtils.clamp(rightMotorOutput, -1.0, 1.0) * maxOutput);
+ m_rightMotor.set(MathUtil.clamp(rightMotorOutput, -1.0, 1.0) * maxOutput);
feed();
}
@@ -251,10 +251,10 @@
m_reported = true;
}
- xSpeed = MathUtils.clamp(xSpeed, -1.0, 1.0);
+ xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
xSpeed = applyDeadband(xSpeed, m_deadband);
- zRotation = MathUtils.clamp(zRotation, -1.0, 1.0);
+ zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
zRotation = applyDeadband(zRotation, m_deadband);
double angularPower;
@@ -263,7 +263,7 @@
if (isQuickTurn) {
if (Math.abs(xSpeed) < m_quickStopThreshold) {
m_quickStopAccumulator = (1 - m_quickStopAlpha) * m_quickStopAccumulator
- + m_quickStopAlpha * MathUtils.clamp(zRotation, -1.0, 1.0) * 2;
+ + m_quickStopAlpha * MathUtil.clamp(zRotation, -1.0, 1.0) * 2;
}
overPower = true;
angularPower = zRotation;
@@ -342,10 +342,10 @@
m_reported = true;
}
- leftSpeed = MathUtils.clamp(leftSpeed, -1.0, 1.0);
+ leftSpeed = MathUtil.clamp(leftSpeed, -1.0, 1.0);
leftSpeed = applyDeadband(leftSpeed, m_deadband);
- rightSpeed = MathUtils.clamp(rightSpeed, -1.0, 1.0);
+ rightSpeed = MathUtil.clamp(rightSpeed, -1.0, 1.0);
rightSpeed = applyDeadband(rightSpeed, m_deadband);
// Square the inputs (while preserving the sign) to increase fine control
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
index d1270bd..45da78a 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java
@@ -16,7 +16,7 @@
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.MathUtils;
+import edu.wpi.first.wpiutil.math.MathUtil;
/**
* A class for driving Killough drive platforms.
@@ -179,10 +179,10 @@
m_reported = true;
}
- ySpeed = MathUtils.clamp(ySpeed, -1.0, 1.0);
+ ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
ySpeed = applyDeadband(ySpeed, m_deadband);
- xSpeed = MathUtils.clamp(xSpeed, -1.0, 1.0);
+ xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
xSpeed = applyDeadband(xSpeed, m_deadband);
// Compensate for gyro angle.
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
index 1d3a878..702bdf5 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
@@ -16,7 +16,7 @@
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.MathUtils;
+import edu.wpi.first.wpiutil.math.MathUtil;
/**
* A class for driving Mecanum drive platforms.
@@ -164,10 +164,10 @@
m_reported = true;
}
- ySpeed = MathUtils.clamp(ySpeed, -1.0, 1.0);
+ ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
ySpeed = applyDeadband(ySpeed, m_deadband);
- xSpeed = MathUtils.clamp(xSpeed, -1.0, 1.0);
+ xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
xSpeed = applyDeadband(xSpeed, m_deadband);
// Compensate for gyro angle.
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
index f3f76a7..1fa2c8d 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
@@ -7,11 +7,25 @@
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;
@@ -140,7 +154,7 @@
*
* @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 .5 degrees since the previous pose update,
+ * 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.
*/
@@ -220,4 +234,38 @@
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
index f697a63..288fa5c 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
@@ -7,12 +7,26 @@
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;
@@ -203,4 +217,35 @@
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/Translation2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
index 49bd3f5..73b0257 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
@@ -7,8 +7,20 @@
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.
@@ -17,6 +29,8 @@
* 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;
@@ -189,4 +203,37 @@
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/kinematics/DifferentialDriveKinematics.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
index 249cea7..06fdd32 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
@@ -15,8 +15,9 @@
* velocity components whereas forward kinematics converts left and right
* component velocities into a linear and angular chassis speed.
*/
+@SuppressWarnings("MemberName")
public class DifferentialDriveKinematics {
- private final double m_trackWidthMeters;
+ public final double trackWidthMeters;
/**
* Constructs a differential drive kinematics object.
@@ -27,7 +28,7 @@
* measured value due to scrubbing effects.
*/
public DifferentialDriveKinematics(double trackWidthMeters) {
- m_trackWidthMeters = trackWidthMeters;
+ this.trackWidthMeters = trackWidthMeters;
}
/**
@@ -41,7 +42,7 @@
return new ChassisSpeeds(
(wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2, 0,
(wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond)
- / m_trackWidthMeters
+ / trackWidthMeters
);
}
@@ -55,9 +56,9 @@
*/
public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
return new DifferentialDriveWheelSpeeds(
- chassisSpeeds.vxMetersPerSecond - m_trackWidthMeters / 2
+ chassisSpeeds.vxMetersPerSecond - trackWidthMeters / 2
* chassisSpeeds.omegaRadiansPerSecond,
- chassisSpeeds.vxMetersPerSecond + m_trackWidthMeters / 2
+ 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
index 0f411ca..2a98f2b 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
@@ -7,7 +7,6 @@
package edu.wpi.first.wpilibj.kinematics;
-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;
@@ -21,103 +20,97 @@
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*
- * <p>Note: It is important to reset both your encoders to zero before you start
- * using this class. Only reset your encoders ONCE. You should not reset your
- * encoders even if you want to reset your robot's pose.
+ * <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 final DifferentialDriveKinematics m_kinematics;
private Pose2d m_poseMeters;
- private double m_prevTimeSeconds = -1;
+ private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
+ private double m_prevLeftDistance;
+ private double m_prevRightDistance;
+
/**
* Constructs a DifferentialDriveOdometry object.
*
- * @param kinematics The differential 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 DifferentialDriveOdometry(DifferentialDriveKinematics kinematics,
+ public DifferentialDriveOdometry(Rotation2d gyroAngle,
Pose2d initialPoseMeters) {
- m_kinematics = kinematics;
m_poseMeters = initialPoseMeters;
+ m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
}
/**
* Constructs a DifferentialDriveOdometry object with the default pose at the origin.
*
- * @param kinematics The differential drive kinematics for your drivetrain.
+ * @param gyroAngle The angle reported by the gyroscope.
*/
- public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics) {
- this(kinematics, new Pose2d());
+ public DifferentialDriveOdometry(Rotation2d gyroAngle) {
+ this(gyroAngle, new Pose2d());
}
/**
- * Resets the robot's position on the field. Do NOT zero your encoders if you
- * call this function at any other time except initialization.
+ * 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) {
+ 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'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.
+ * 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 currentTimeSeconds The current time in seconds.
- * @param angle The current robot angle.
- * @param wheelSpeeds The current wheel speeds.
+ * @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 updateWithTime(double currentTimeSeconds, Rotation2d angle,
- DifferentialDriveWheelSpeeds wheelSpeeds) {
- double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
- m_prevTimeSeconds = currentTimeSeconds;
+ public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters,
+ double rightDistanceMeters) {
+ double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
+ double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
- var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
+ 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(chassisState.vxMetersPerSecond * period,
- chassisState.vyMetersPerSecond * period,
- angle.minus(m_previousAngle).getRadians()));
+ new Twist2d(averageDeltaDistance, 0.0, 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 angle The angle of the robot.
- * @param wheelSpeeds The current wheel speeds.
- * @return The new pose of the robot.
- */
- public Pose2d update(Rotation2d angle,
- DifferentialDriveWheelSpeeds wheelSpeeds) {
- return updateWithTime(Timer.getFPGATimestamp(),
- angle, wheelSpeeds);
- }
}
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..af4a7f1
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+ }
+}
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
index 3ea8fff..22ef83a 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
@@ -13,7 +13,7 @@
import edu.wpi.first.wpilibj.geometry.Twist2d;
/**
- * Class for mecnaum drive odometry. Odometry allows you to track the robot's
+ * 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.
*
@@ -26,17 +26,21 @@
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, Pose2d initialPoseMeters) {
+ 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();
}
@@ -44,23 +48,30 @@
* 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) {
- this(kinematics, new Pose2d());
+ 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) {
+ 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() {
@@ -76,15 +87,17 @@
* angular rate that is calculated from forward kinematics.
*
* @param currentTimeSeconds The current time in seconds.
- * @param angle The angle of the robot.
+ * @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 angle,
+ 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,
@@ -104,13 +117,13 @@
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
- * @param angle THe angle of the robot.
+ * @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 angle,
+ public Pose2d update(Rotation2d gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
- return updateWithTime(Timer.getFPGATimestamp(), angle,
+ return updateWithTime(Timer.getFPGATimestamp(), gyroAngle,
wheelSpeeds);
}
}
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
index 2cfea4e..c08070c 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
@@ -26,17 +26,21 @@
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, Pose2d initialPose) {
+ 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();
}
@@ -44,23 +48,30 @@
* 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) {
- this(kinematics, new Pose2d());
+ public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
+ this(kinematics, gyroAngle, new Pose2d());
}
/**
* Resets the robot's position on the field.
*
- * @param pose The position on the field that your robot is at.
+ * <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) {
+ 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() {
@@ -76,17 +87,19 @@
* angular rate that is calculated from forward kinematics.
*
* @param currentTimeSeconds The current time in seconds.
- * @param angle The angle of the robot.
+ * @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 angle,
+ 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,
@@ -107,13 +120,13 @@
* also takes in an angle parameter which is used instead of the angular
* rate that is calculated from forward kinematics.
*
- * @param angle The angle of the robot.
+ * @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 angle, SwerveModuleState... moduleStates) {
- return updateWithTime(Timer.getFPGATimestamp(), angle, moduleStates);
+ public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
+ return updateWithTime(Timer.getFPGATimestamp(), gyroAngle, moduleStates);
}
}
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
index f545b7d..5a4fb36 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
@@ -11,7 +11,6 @@
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.Sendable;
-import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
@@ -64,17 +63,13 @@
startLiveWindow = enabled;
liveWindowEnabled = enabled;
updateValues(); // Force table generation now to make sure everything is defined
- Scheduler scheduler = Scheduler.getInstance();
if (enabled) {
System.out.println("Starting live window mode.");
- scheduler.disable();
- scheduler.removeAll();
} else {
System.out.println("stopping live window mode.");
SendableRegistry.foreachLiveWindow(dataHandle, cbdata -> {
cbdata.builder.stopLiveWindowMode();
});
- scheduler.enable();
}
enabledEntry.setBoolean(enabled);
}
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
index c16ee99..c88256a 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/Shuffleboard.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/Shuffleboard.java
@@ -16,7 +16,7 @@
* 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>For example, displaying a boolean entry with a toggle button:</p>
* <pre>{@code
* NetworkTableEntry myBoolean = Shuffleboard.getTab("Example Tab")
* .add("My Boolean", false)
@@ -24,7 +24,7 @@
* .getEntry();
* }</pre>
*
- * Changing the colors of the boolean box:
+ * <p>Changing the colors of the boolean box:</p>
* <pre>{@code
* NetworkTableEntry myBoolean = Shuffleboard.getTab("Example Tab")
* .add("My Boolean", false)
@@ -33,8 +33,8 @@
* .getEntry();
* }</pre>
*
- * 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>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")
@@ -42,7 +42,6 @@
* .withWidget("Toggle Button")
* .getEntry();
* }</pre>
- * </p>
*
* <p>Teams are encouraged to set up shuffleboard layouts at the start of the robot program.</p>
*/
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
index 57435b4..cdf1ffb 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java
@@ -16,7 +16,6 @@
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpilibj.Sendable;
-import edu.wpi.first.wpilibj.command.Command;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
@@ -25,7 +24,7 @@
* {@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 {@link Command} you want to run as an autonomous into a {@link
+ * 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.
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
index 79b9b2a..a8cd557 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java
@@ -8,7 +8,9 @@
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;
@@ -454,6 +456,10 @@
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
@@ -467,7 +473,9 @@
public static synchronized void foreachLiveWindow(int dataHandle,
Consumer<CallbackData> callback) {
CallbackData cbdata = new CallbackData();
- for (Component comp : components.values()) {
+ foreachComponents.clear();
+ foreachComponents.addAll(components.values());
+ for (Component comp : foreachComponents) {
if (comp.m_sendable == null) {
continue;
}
@@ -508,5 +516,6 @@
}
}
}
+ foreachComponents.clear();
}
}
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
index 5674231..578787c 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
@@ -50,10 +50,21 @@
// 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));
- m_coefficients.set(4, i, m_coefficients.get(2, i) * (3 - i));
- m_coefficients.set(5, i, m_coefficients.get(3, 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));
}
}
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
index 5621341..3b2d3eb 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
@@ -47,13 +47,24 @@
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.
- // Then populate row 4 and 5 with the second derivatives.
+ // 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));
- m_coefficients.set(4, i, m_coefficients.get(2, i) * (5 - i));
- m_coefficients.set(5, i, m_coefficients.get(3, 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));
}
}
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
index 1c71229..a419bf9 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
@@ -89,7 +89,7 @@
* provided waypoints and control vectors.
*/
@SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength",
- "PMD.AvoidInstantiatingObjectsInLoops"})
+ "PMD.AvoidInstantiatingObjectsInLoops"})
public static CubicHermiteSpline[] getCubicSplinesFromControlVectors(
Spline.ControlVector start, Translation2d[] waypoints, Spline.ControlVector end) {
@@ -108,19 +108,30 @@
System.arraycopy(waypoints, 0, newWaypts, 1, waypoints.length);
newWaypts[newWaypts.length - 1] = new Translation2d(xFinal[0], yFinal[0]);
- final double[] a = new double[1 + newWaypts.length - 3];
+ // 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);
- final double[] c = new double[1 + newWaypts.length - 3];
+ // Below-diagonal of tridiagonal matrix, zero-padded
+ final double[] c = new double[newWaypts.length - 2];
- final double[] dx = new double[2 + newWaypts.length - 4];
- final double[] dy = new double[2 + newWaypts.length - 4];
+ // 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;
@@ -128,13 +139,14 @@
}
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; i++) {
- dx[i] = newWaypts[i + 1].getX() - newWaypts[i - 1].getX();
- dy[i] = newWaypts[i + 1].getY() - newWaypts[i - 1].getY();
+ 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());
}
}
@@ -143,6 +155,7 @@
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);
@@ -175,15 +188,15 @@
/ 4.0;
double[] midXControlVector = {waypoints[0].getX(), xDeriv};
- double[] midYControlVector = {waypoints[0].getX(), yDeriv};
+ double[] midYControlVector = {waypoints[0].getY(), yDeriv};
splines[0] = new CubicHermiteSpline(xInitial, midXControlVector,
- yInitial, midYControlVector);
+ yInitial, midYControlVector);
splines[1] = new CubicHermiteSpline(midXControlVector, xFinal,
- midYControlVector, yFinal);
+ midYControlVector, yFinal);
} else {
splines[0] = new CubicHermiteSpline(xInitial, xFinal,
- yInitial, yFinal);
+ yInitial, yFinal);
}
return splines;
}
@@ -207,7 +220,7 @@
var yInitial = controlVectors[i].y;
var yFinal = controlVectors[i + 1].y;
splines[i] = new QuinticHermiteSpline(xInitial, xFinal,
- yInitial, yFinal);
+ yInitial, yFinal);
}
return splines;
}
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
index 934602a..4f49244 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
@@ -8,8 +8,11 @@
package edu.wpi.first.wpilibj.trajectory;
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;
/**
@@ -137,18 +140,23 @@
@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() {
@@ -228,6 +236,29 @@
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
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
index 7bff2cc..fbc5ddb 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java
@@ -11,7 +11,11 @@
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;
/**
@@ -59,7 +63,7 @@
* @param constraints List of user-defined constraints.
* @return Instance of the current config object.
*/
- public TrajectoryConfig addConstraints(List<TrajectoryConstraint> constraints) {
+ public TrajectoryConfig addConstraints(List<? extends TrajectoryConstraint> constraints) {
m_constraints.addAll(constraints);
return this;
}
@@ -77,10 +81,34 @@
}
/**
- * Returns the starting velocity of the trajectory.
- *
- * @return The starting velocity of the trajectory.
- */
+ * 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;
}
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
index aa3840a..a4d6898 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
@@ -8,6 +8,7 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.ArrayList;
+import java.util.Collection;
import java.util.List;
import edu.wpi.first.wpilibj.geometry.Pose2d;
@@ -160,8 +161,7 @@
@SuppressWarnings("LocalVariableName")
public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config) {
var originalList = SplineHelper.getQuinticControlVectorsFromWaypoints(waypoints);
- var newList = new ControlVectorList();
- newList.addAll(originalList);
+ var newList = new ControlVectorList(originalList);
return generateTrajectory(newList, config);
}
@@ -194,6 +194,17 @@
}
// Work around type erasure signatures
- private static class ControlVectorList extends ArrayList<Spline.ControlVector> {
+ 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/TrajectoryUtil.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
new file mode 100644
index 0000000..f4b1b12
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.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.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 {
+ 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/constraint/DifferentialDriveKinematicsConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
index 274bdfb..9a57720 100644
--- 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
@@ -24,6 +24,7 @@
/**
* 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,
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..d567009
--- /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 mecanum 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/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/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/controller/PIDInputOutputTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java
index 67222d3..873d03f 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java
@@ -43,7 +43,7 @@
double out = 0;
for (int i = 0; i < 5; i++) {
- out = m_controller.calculate(.025, 0);
+ out = m_controller.calculate(0.025, 0);
}
assertEquals(-0.5 * m_controller.getPeriod(), out, 1e-5);
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
index 4562ed2..aabc455 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
@@ -17,30 +17,13 @@
class DifferentialDriveOdometryTest {
private static final double kEpsilon = 1E-9;
- private final DifferentialDriveKinematics m_kinematics
- = new DifferentialDriveKinematics(0.381 * 2);
- private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(m_kinematics);
+ private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(
+ new Rotation2d());
@Test
- void testOneIteration() {
- m_odometry.resetPosition(new Pose2d());
- var speeds = new DifferentialDriveWheelSpeeds(0.02, 0.02);
- m_odometry.updateWithTime(0.0, new Rotation2d(), new DifferentialDriveWheelSpeeds());
- var pose = m_odometry.updateWithTime(1.0, new Rotation2d(), speeds);
-
- assertAll(
- () -> assertEquals(0.02, pose.getTranslation().getX(), kEpsilon),
- () -> assertEquals(0.00, pose.getTranslation().getY(), kEpsilon),
- () -> assertEquals(0.00, pose.getRotation().getRadians(), kEpsilon)
- );
- }
-
- @Test
- void testQuarterCircle() {
- m_odometry.resetPosition(new Pose2d());
- var speeds = new DifferentialDriveWheelSpeeds(0.0, 5 * Math.PI);
- m_odometry.updateWithTime(0.0, new Rotation2d(), new DifferentialDriveWheelSpeeds());
- var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), speeds);
+ 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),
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
index 04acceb..4619bf0 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
@@ -9,6 +9,7 @@
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;
@@ -21,10 +22,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);
+ private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
+ new Rotation2d());
@Test
void testMultipleConsecutiveUpdates() {
@@ -71,4 +74,21 @@
);
}
+ @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/SwerveDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
index c945b8b..85b832c 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
@@ -9,6 +9,7 @@
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;
@@ -24,7 +25,8 @@
private final SwerveDriveKinematics m_kinematics =
new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
- private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics);
+ private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
+ new Rotation2d());
@Test
void testTwoIterations() {
@@ -73,4 +75,22 @@
() -> 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/spline/CubicHermiteSplineTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
index d7d4558..5a8349d 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
@@ -76,6 +76,20 @@
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(),
@@ -104,4 +118,31 @@
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);
+ }
}
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
index 19b9374..5b45e3e 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
@@ -23,7 +23,7 @@
import static org.junit.jupiter.api.Assertions.assertTrue;
class TrajectoryGeneratorTest {
- static Trajectory getTrajectory(List<TrajectoryConstraint> constraints) {
+ static Trajectory getTrajectory(List<? extends TrajectoryConstraint> constraints) {
final double maxVelocity = feetToMeters(12.0);
final double maxAccel = feetToMeters(12);
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/wpilibjExamples/build.gradle b/wpilibjExamples/build.gradle
index 7b65086..651d8e5 100644
--- a/wpilibjExamples/build.gradle
+++ b/wpilibjExamples/build.gradle
@@ -11,13 +11,15 @@
dependencies {
- compile project(':wpilibj')
+ implementation project(':wpilibj')
- compile project(':hal')
- compile project(':wpiutil')
- compile project(':ntcore')
- compile project(':cscore')
- compile project(':cameraserver')
+ implementation project(':hal')
+ implementation project(':wpiutil')
+ implementation project(':ntcore')
+ implementation project(':cscore')
+ implementation project(':cameraserver')
+ implementation project(':wpilibOldCommands')
+ implementation project(':wpilibNewCommands')
}
if (!project.hasProperty('skipPMD')) {
@@ -48,4 +50,7 @@
commandFile = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/commands/commands.json")
}
+ext {
+ isCppCommands = false
+}
apply from: "${rootDir}/shared/examplecheck.gradle"
diff --git a/wpilibjExamples/publish.gradle b/wpilibjExamples/publish.gradle
index a2c0746..a5ad1c6 100644
--- a/wpilibjExamples/publish.gradle
+++ b/wpilibjExamples/publish.gradle
@@ -12,8 +12,8 @@
def outputsFolder = file("$project.buildDir/outputs")
task javaExamplesZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = examplesZipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = examplesZipBaseName
from(licenseFile) {
into '/'
@@ -25,8 +25,8 @@
}
task javaTemplatesZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = templatesZipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = templatesZipBaseName
from(licenseFile) {
into '/'
@@ -38,8 +38,8 @@
}
task javaCommandsZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = commandsZipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = commandsZipBaseName
from(licenseFile) {
into '/'
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Main.java
similarity index 88%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Main.java
index d7f6e26..2feaca2 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Main.java
@@ -1,29 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* 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.templates.iterative;
-
-import edu.wpi.first.wpilibj.RobotBase;
-
-/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
- */
-public final class Main {
- private Main() {
- }
-
- /**
- * Main initialization function. Do not perform any initialization here.
- *
- * <p>If you change your main robot class, change the parameter type.
- */
- public static void main(String... args) {
- RobotBase.startRobot(Robot::new);
- }
-}
+/*----------------------------------------------------------------------------*/
+/* 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.examples.addressableled;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+ private Main() {
+ }
+
+ /**
+ * Main initialization function. Do not perform any initialization here.
+ *
+ * <p>If you change your main robot class, change the parameter type.
+ */
+ public static void main(String... args) {
+ RobotBase.startRobot(Robot::new);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java
new file mode 100644
index 0000000..b89365b
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.addressableled;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.TimedRobot;
+
+public class Robot extends TimedRobot {
+ private AddressableLED m_led;
+ private AddressableLEDBuffer m_ledBuffer;
+ // Store what the last hue of the first pixel is
+ private int m_rainbowFirstPixelHue;
+
+ @Override
+ public void robotInit() {
+ // PWM port 0
+ // Must be a PWM header, not MXP or DIO
+ m_led = new AddressableLED(0);
+
+ // Reuse buffer
+ // Default to a length of 60, start empty output
+ // Length is expensive to set, so only set it once, then just update data
+ m_ledBuffer = new AddressableLEDBuffer(60);
+ m_led.setLength(m_ledBuffer.getLength());
+
+ // Set the data
+ m_led.setData(m_ledBuffer);
+ m_led.start();
+ }
+
+ @Override
+ public void robotPeriodic() {
+ // Fill the buffer with a rainbow
+ rainbow();
+ // Set the LEDs
+ m_led.setData(m_ledBuffer);
+ }
+
+ private void rainbow() {
+ // For every pixel
+ for (var i = 0; i < m_ledBuffer.getLength(); i++) {
+ // Calculate the hue - hue is easier for rainbows because the color
+ // shape is a circle so only one value needs to precess
+ final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_ledBuffer.getLength())) % 180;
+ // Set the value
+ m_ledBuffer.setHSV(i, hue, 255, 128);
+ }
+ // Increase by to make the rainbow "move"
+ m_rainbowFirstPixelHue += 3;
+ // Check bounds
+ m_rainbowFirstPixelHue %= 180;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Main.java
similarity index 88%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Main.java
index d7f6e26..fb5c2ab 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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.templates.iterative;
+package edu.wpi.first.wpilibj.examples.arcadedrivexboxcontroller;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java
new file mode 100644
index 0000000..915d77e
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.arcadedrivexboxcontroller;
+
+import edu.wpi.first.wpilibj.GenericHID.Hand;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class.
+ * Runs the motors with split arcade steering and an Xbox controller.
+ */
+public class Robot extends TimedRobot {
+ private final PWMVictorSPX m_leftMotor = new PWMVictorSPX(0);
+ private final PWMVictorSPX m_rightMotor = new PWMVictorSPX(1);
+ private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final XboxController m_driverController = new XboxController(0);
+
+ @Override
+ public void teleopPeriodic() {
+ // Drive with split arcade drive.
+ // That means that the Y axis of the left stick moves forward
+ // and backward, and the X of the right stick turns left and right.
+ m_robotDrive.arcadeDrive(m_driverController.getY(Hand.kLeft),
+ m_driverController.getX(Hand.kRight));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java
new file mode 100644
index 0000000..fda1290
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.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.examples.armbot;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kLeftMotor1Port = 0;
+ public static final int kLeftMotor2Port = 1;
+ public static final int kRightMotor1Port = 2;
+ public static final int kRightMotor2Port = 3;
+
+ public static final int[] kLeftEncoderPorts = new int[]{0, 1};
+ public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final boolean kLeftEncoderReversed = false;
+ public static final boolean kRightEncoderReversed = true;
+
+ public static final int kEncoderCPR = 1024;
+ public static final double kWheelDiameterInches = 6;
+ public static final double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
+ }
+
+ public static final class ArmConstants {
+ public static final int kMotorPort = 4;
+
+ public static final double kP = 1;
+
+ // These are fake gains; in actuality these must be determined individually for each robot
+ public static final double kSVolts = 1;
+ public static final double kCosVolts = 1;
+ public static final double kVVoltSecondPerRad = 0.5;
+ public static final double kAVoltSecondSquaredPerRad = 0.1;
+
+ public static final double kMaxVelocityRadPerSecond = 3;
+ public static final double kMaxAccelerationRadPerSecSquared = 10;
+
+ public static final int[] kEncoderPorts = new int[]{4, 5};
+ public static final int kEncoderPPR = 256;
+ public static final double kEncoderDistancePerPulse = 2.0 * Math.PI / kEncoderPPR;
+
+ // The offset of the arm from the horizontal in its neutral position,
+ // measured from the horizontal
+ public static final double kArmOffsetRads = 0.5;
+ }
+
+ public static final class AutoConstants {
+ public static final double kAutoTimeoutSeconds = 12;
+ public static final double kAutoShootTimeSeconds = 7;
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 1;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java
similarity index 76%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java
index d7f6e26..681a38e 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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.templates.iterative;
+package edu.wpi.first.wpilibj.examples.armbot;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java
new file mode 100644
index 0000000..a171fc6
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.armbot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
new file mode 100644
index 0000000..64f44d6
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.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.examples.armbot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.armbot.subsystems.ArmSubsystem;
+import edu.wpi.first.wpilibj.examples.armbot.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems
+ private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+ private final ArmSubsystem m_robotArm = new ArmSubsystem();
+
+ // The driver's controller
+ XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+
+ // Configure default commands
+ // Set the default drive command to split-stick arcade drive
+ m_robotDrive.setDefaultCommand(
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ new RunCommand(() -> m_robotDrive
+ .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
+ m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
+
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+ * {@link JoystickButton}.
+ */
+ private void configureButtonBindings() {
+
+ // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
+ new JoystickButton(m_driverController, Button.kA.value)
+ .whenPressed(() -> m_robotArm.setGoal(2), m_robotArm);
+
+ // Move the arm to neutral position when the 'B' button is pressed.
+ new JoystickButton(m_driverController, Button.kB.value)
+ .whenPressed(() -> m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads), m_robotArm);
+
+ // Drive at half speed when the bumper is held
+ new JoystickButton(m_driverController, Button.kBumperRight.value)
+ .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
+ .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+ }
+
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ return new InstantCommand();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
new file mode 100644
index 0000000..7d549e5
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.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.examples.armbot.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.controller.ArmFeedforward;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kAVoltSecondSquaredPerRad;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kArmOffsetRads;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kCosVolts;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kMaxAccelerationRadPerSecSquared;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kMaxVelocityRadPerSecond;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kMotorPort;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kP;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kSVolts;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kVVoltSecondPerRad;
+
+/**
+ * A robot arm subsystem that moves with a motion profile.
+ */
+public class ArmSubsystem extends ProfiledPIDSubsystem {
+ private final PWMVictorSPX m_motor = new PWMVictorSPX(kMotorPort);
+ private final Encoder m_encoder = new Encoder(kEncoderPorts[0], kEncoderPorts[1]);
+ private final ArmFeedforward m_feedforward =
+ new ArmFeedforward(kSVolts, kCosVolts, kVVoltSecondPerRad, kAVoltSecondSquaredPerRad);
+
+ /**
+ * Create a new ArmSubsystem.
+ */
+ public ArmSubsystem() {
+ super(new ProfiledPIDController(
+ kP,
+ 0,
+ 0,
+ new TrapezoidProfile.Constraints(kMaxVelocityRadPerSecond,
+ kMaxAccelerationRadPerSecSquared)));
+ m_encoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ // Start arm at rest in neutral position
+ setGoal(kArmOffsetRads);
+ }
+
+ @Override
+ public void useOutput(double output, TrapezoidProfile.State setpoint) {
+ // Calculate the feedforward from the sepoint
+ double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
+ // Add the feedforward to the PID output to get the motor output
+ m_motor.setVoltage(output + feedforward);
+ }
+
+ @Override
+ public double getMeasurement() {
+ return m_encoder.getDistance() + kArmOffsetRads;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000..a22079e
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.armbot.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftMotor1Port;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftMotor2Port;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightMotor1Port;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightMotor2Port;
+
+public class DriveSubsystem extends SubsystemBase {
+ // The motors on the left side of the drive.
+ private final SpeedControllerGroup m_leftMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
+ new PWMVictorSPX(kLeftMotor2Port));
+
+ // The motors on the right side of the drive.
+ private final SpeedControllerGroup m_rightMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
+ new PWMVictorSPX(kRightMotor2Port));
+
+ // The robot's drive
+ private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+
+ // The left-side drive encoder
+ private final Encoder m_leftEncoder =
+ new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
+
+ // The right-side drive encoder
+ private final Encoder m_rightEncoder =
+ new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+
+ /**
+ * Creates a new DriveSubsystem.
+ */
+ public DriveSubsystem() {
+ // Sets the distance per pulse for the encoders
+ m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ }
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ public void arcadeDrive(double fwd, double rot) {
+ m_drive.arcadeDrive(fwd, rot);
+ }
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ public void resetEncoders() {
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+ }
+
+ /**
+ * Gets the average distance of the two encoders.
+ *
+ * @return the average of the two encoder readings
+ */
+ public double getAverageEncoderDistance() {
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
+ }
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ public Encoder getLeftEncoder() {
+ return m_leftEncoder;
+ }
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ public Encoder getRightEncoder() {
+ return m_rightEncoder;
+ }
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_drive.setMaxOutput(maxOutput);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java
new file mode 100644
index 0000000..baa15b6
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.armbotoffboard;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kLeftMotor1Port = 0;
+ public static final int kLeftMotor2Port = 1;
+ public static final int kRightMotor1Port = 2;
+ public static final int kRightMotor2Port = 3;
+
+ public static final int[] kLeftEncoderPorts = new int[]{0, 1};
+ public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final boolean kLeftEncoderReversed = false;
+ public static final boolean kRightEncoderReversed = true;
+
+ public static final int kEncoderCPR = 1024;
+ public static final double kWheelDiameterInches = 6;
+ public static final double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
+ }
+
+ public static final class ArmConstants {
+ public static final int kMotorPort = 4;
+
+ public static final double kP = 1;
+
+ // These are fake gains; in actuality these must be determined individually for each robot
+ public static final double kSVolts = 1;
+ public static final double kCosVolts = 1;
+ public static final double kVVoltSecondPerRad = 0.5;
+ public static final double kAVoltSecondSquaredPerRad = 0.1;
+
+ public static final double kMaxVelocityRadPerSecond = 3;
+ public static final double kMaxAccelerationRadPerSecSquared = 10;
+
+ // The offset of the arm from the horizontal in its neutral position,
+ // measured from the horizontal
+ public static final double kArmOffsetRads = 0.5;
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 1;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
new file mode 100644
index 0000000..0fa7dae
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.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.examples.armbotoffboard;
+
+import edu.wpi.first.wpilibj.SpeedController;
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor controller.
+ *
+ * <p>Has no actual functionality.
+ */
+public class ExampleSmartMotorController implements SpeedController {
+ public enum PIDMode {
+ kPosition,
+ kVelocity,
+ kMovementWitchcraft
+ }
+
+ /**
+ * Creates a new ExampleSmartMotorController.
+ *
+ * @param port The port for the controller.
+ */
+ @SuppressWarnings("PMD.UnusedFormalParameter")
+ public ExampleSmartMotorController(int port) {
+ }
+
+ /**
+ * Example method for setting the PID gains of the smart controller.
+ *
+ * @param kp The proportional gain.
+ * @param ki The integral gain.
+ * @param kd The derivative gain.
+ */
+ public void setPID(double kp, double ki, double kd) {
+ }
+
+ /**
+ * Example method for setting the setpoint of the smart controller in PID mode.
+ *
+ * @param mode The mode of the PID controller.
+ * @param setpoint The controller setpoint.
+ * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+ */
+ public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {
+ }
+
+ /**
+ * Places this motor controller in follower mode.
+ *
+ * @param master The master to follow.
+ */
+ public void follow(ExampleSmartMotorController master) {
+ }
+
+ /**
+ * Returns the encoder distance.
+ *
+ * @return The current encoder distance.
+ */
+ public double getEncoderDistance() {
+ return 0;
+ }
+
+ /**
+ * Returns the encoder rate.
+ *
+ * @return The current encoder rate.
+ */
+ public double getEncoderRate() {
+ return 0;
+ }
+
+ /**
+ * Resets the encoder to zero distance.
+ */
+ public void resetEncoder() {
+ }
+
+ @Override
+ public void set(double speed) {
+ }
+
+ @Override
+ public double get() {
+ return 0;
+ }
+
+ @Override
+ public void setInverted(boolean isInverted) {
+
+ }
+
+ @Override
+ public boolean getInverted() {
+ return false;
+ }
+
+ @Override
+ public void disable() {
+ }
+
+ @Override
+ public void stopMotor() {
+ }
+
+ @Override
+ public void pidWrite(double output) {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java
similarity index 76%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java
index d7f6e26..ff9e928 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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.templates.iterative;
+package edu.wpi.first.wpilibj.examples.armbotoffboard;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java
new file mode 100644
index 0000000..8a7f99b
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.armbotoffboard;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java
new file mode 100644
index 0000000..285bd58
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.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.examples.armbotoffboard;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.ArmSubsystem;
+import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems
+ private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+ private final ArmSubsystem m_robotArm = new ArmSubsystem();
+
+ // The driver's controller
+ XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+
+ // Configure default commands
+ // Set the default drive command to split-stick arcade drive
+ m_robotDrive.setDefaultCommand(
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ new RunCommand(() -> m_robotDrive
+ .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
+ m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
+
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+ * {@link JoystickButton}.
+ */
+ private void configureButtonBindings() {
+
+ // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
+ new JoystickButton(m_driverController, Button.kA.value)
+ .whenPressed(() -> m_robotArm.setGoal(2), m_robotArm);
+
+ // Move the arm to neutral position when the 'B' button is pressed.
+ new JoystickButton(m_driverController, Button.kB.value)
+ .whenPressed(() -> m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads), m_robotArm);
+
+ // Drive at half speed when the bumper is held
+ new JoystickButton(m_driverController, Button.kBumperRight.value)
+ .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
+ .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+ }
+
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ return new InstantCommand();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
new file mode 100644
index 0000000..2d2698c
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.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.examples.armbotoffboard.subsystems;
+
+import edu.wpi.first.wpilibj.controller.ArmFeedforward;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;
+
+import edu.wpi.first.wpilibj.examples.armbotoffboard.ExampleSmartMotorController;
+
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kAVoltSecondSquaredPerRad;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kArmOffsetRads;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kCosVolts;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kMaxAccelerationRadPerSecSquared;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kMaxVelocityRadPerSecond;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kMotorPort;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kP;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kSVolts;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kVVoltSecondPerRad;
+
+/**
+ * A robot arm subsystem that moves with a motion profile.
+ */
+public class ArmSubsystem extends TrapezoidProfileSubsystem {
+ private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(kMotorPort);
+ private final ArmFeedforward m_feedforward =
+ new ArmFeedforward(kSVolts, kCosVolts, kVVoltSecondPerRad, kAVoltSecondSquaredPerRad);
+
+ /**
+ * Create a new ArmSubsystem.
+ */
+ public ArmSubsystem() {
+ super(new TrapezoidProfile.Constraints(kMaxVelocityRadPerSecond,
+ kMaxAccelerationRadPerSecSquared),
+ kArmOffsetRads);
+ m_motor.setPID(kP, 0, 0);
+ }
+
+ @Override
+ public void useState(TrapezoidProfile.State setpoint) {
+ // Calculate the feedforward from the sepoint
+ double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
+ // Add the feedforward to the PID output to get the motor output
+ m_motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition,
+ setpoint.position,
+ feedforward / 12.0);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000..974f514
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.armbotoffboard.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftMotor1Port;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftMotor2Port;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightMotor1Port;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightMotor2Port;
+
+public class DriveSubsystem extends SubsystemBase {
+ // The motors on the left side of the drive.
+ private final SpeedControllerGroup m_leftMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
+ new PWMVictorSPX(kLeftMotor2Port));
+
+ // The motors on the right side of the drive.
+ private final SpeedControllerGroup m_rightMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
+ new PWMVictorSPX(kRightMotor2Port));
+
+ // The robot's drive
+ private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+
+ // The left-side drive encoder
+ private final Encoder m_leftEncoder =
+ new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
+
+ // The right-side drive encoder
+ private final Encoder m_rightEncoder =
+ new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+
+ /**
+ * Creates a new DriveSubsystem.
+ */
+ public DriveSubsystem() {
+ // Sets the distance per pulse for the encoders
+ m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ }
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ public void arcadeDrive(double fwd, double rot) {
+ m_drive.arcadeDrive(fwd, rot);
+ }
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ public void resetEncoders() {
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+ }
+
+ /**
+ * Gets the average distance of the two encoders.
+ *
+ * @return the average of the two encoder readings
+ */
+ public double getAverageEncoderDistance() {
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
+ }
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ public Encoder getLeftEncoder() {
+ return m_leftEncoder;
+ }
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ public Encoder getRightEncoder() {
+ return m_rightEncoder;
+ }
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_drive.setMaxOutput(maxOutput);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
index 9f33156..cc3d2e1 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
@@ -9,7 +9,8 @@
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -29,13 +30,13 @@
private static final double kWheelRadius = 0.0508; // meters
private static final int kEncoderResolution = 4096;
- private final Spark m_leftMaster = new Spark(1);
- private final Spark m_leftFollower = new Spark(2);
- private final Spark m_rightMaster = new Spark(3);
- private final Spark m_rightFollower = new Spark(4);
+ private final SpeedController m_leftMaster = new PWMVictorSPX(1);
+ private final SpeedController m_leftFollower = new PWMVictorSPX(2);
+ private final SpeedController m_rightMaster = new PWMVictorSPX(3);
+ private final SpeedController m_rightFollower = new PWMVictorSPX(4);
private final Encoder m_leftEncoder = new Encoder(0, 1);
- private final Encoder m_rightEncoder = new Encoder(0, 1);
+ private final Encoder m_rightEncoder = new Encoder(2, 3);
private final SpeedControllerGroup m_leftGroup
= new SpeedControllerGroup(m_leftMaster, m_leftFollower);
@@ -50,8 +51,7 @@
private final DifferentialDriveKinematics m_kinematics
= new DifferentialDriveKinematics(kTrackWidth);
- private final DifferentialDriveOdometry m_odometry
- = new DifferentialDriveOdometry(m_kinematics);
+ private final DifferentialDriveOdometry m_odometry;
/**
* Constructs a differential drive object.
@@ -65,6 +65,11 @@
// resolution.
m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+
+ m_odometry = new DifferentialDriveOdometry(getAngle());
}
/**
@@ -78,15 +83,6 @@
}
/**
- * Returns the current wheel speeds.
- *
- * @return The current wheel speeds.
- */
- public DifferentialDriveWheelSpeeds getCurrentSpeeds() {
- return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
- }
-
- /**
* Sets the desired wheel speeds.
*
* @param speeds The desired wheel speeds.
@@ -116,6 +112,6 @@
* Updates the field-relative position.
*/
public void updateOdometry() {
- m_odometry.update(getAngle(), getCurrentSpeeds());
+ m_odometry.update(getAngle(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java
new file mode 100644
index 0000000..5f4bbeb
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.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.examples.drivedistanceoffboard;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kLeftMotor1Port = 0;
+ public static final int kLeftMotor2Port = 1;
+ public static final int kRightMotor1Port = 2;
+ public static final int kRightMotor2Port = 3;
+
+ // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+ // These characterization values MUST be determined either experimentally or theoretically
+ // for *your* robot's drive.
+ // The RobotPy Characterization Toolsuite provides a convenient tool for obtaining these
+ // values for your robot.
+ public static final double ksVolts = 1;
+ public static final double kvVoltSecondsPerMeter = 0.8;
+ public static final double kaVoltSecondsSquaredPerMeter = 0.15;
+
+ public static final double kp = 1;
+
+ public static final double kMaxSpeedMetersPerSecond = 3;
+ public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 1;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java
new file mode 100644
index 0000000..6e19020
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.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.examples.drivedistanceoffboard;
+
+import edu.wpi.first.wpilibj.SpeedController;
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor controller.
+ *
+ * <p>Has no actual functionality.
+ */
+public class ExampleSmartMotorController implements SpeedController {
+ public enum PIDMode {
+ kPosition,
+ kVelocity,
+ kMovementWitchcraft
+ }
+
+ /**
+ * Creates a new ExampleSmartMotorController.
+ *
+ * @param port The port for the controller.
+ */
+ @SuppressWarnings("PMD.UnusedFormalParameter")
+ public ExampleSmartMotorController(int port) {
+ }
+
+ /**
+ * Example method for setting the PID gains of the smart controller.
+ *
+ * @param kp The proportional gain.
+ * @param ki The integral gain.
+ * @param kd The derivative gain.
+ */
+ public void setPID(double kp, double ki, double kd) {
+ }
+
+ /**
+ * Example method for setting the setpoint of the smart controller in PID mode.
+ *
+ * @param mode The mode of the PID controller.
+ * @param setpoint The controller setpoint.
+ * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+ */
+ public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {
+ }
+
+ /**
+ * Places this motor controller in follower mode.
+ *
+ * @param master The master to follow.
+ */
+ public void follow(ExampleSmartMotorController master) {
+ }
+
+ /**
+ * Returns the encoder distance.
+ *
+ * @return The current encoder distance.
+ */
+ public double getEncoderDistance() {
+ return 0;
+ }
+
+ /**
+ * Returns the encoder rate.
+ *
+ * @return The current encoder rate.
+ */
+ public double getEncoderRate() {
+ return 0;
+ }
+
+ /**
+ * Resets the encoder to zero distance.
+ */
+ public void resetEncoder() {
+ }
+
+ @Override
+ public void set(double speed) {
+ }
+
+ @Override
+ public double get() {
+ return 0;
+ }
+
+ @Override
+ public void setInverted(boolean isInverted) {
+
+ }
+
+ @Override
+ public boolean getInverted() {
+ return false;
+ }
+
+ @Override
+ public void disable() {
+ }
+
+ @Override
+ public void stopMotor() {
+ }
+
+ @Override
+ public void pidWrite(double output) {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java
similarity index 75%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java
index d7f6e26..89ab553 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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.templates.iterative;
+package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java
new file mode 100644
index 0000000..e8ac285
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.drivedistanceoffboard;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
new file mode 100644
index 0000000..a0eb86a
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.drivedistanceoffboard;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands.DriveDistanceProfiled;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxSpeedMetersPerSecond;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems
+ private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+
+ // The driver's controller
+ XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+
+ // Configure default commands
+ // Set the default drive command to split-stick arcade drive
+ m_robotDrive.setDefaultCommand(
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ new RunCommand(() -> m_robotDrive
+ .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
+ m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
+
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+ * {@link JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of 10 seconds
+ new JoystickButton(m_driverController, Button.kA.value)
+ .whenPressed(new DriveDistanceProfiled(3, m_robotDrive).withTimeout(10));
+
+ // Do the same thing as above when the 'B' button is pressed, but defined inline
+ new JoystickButton(m_driverController, Button.kB.value)
+ .whenPressed(
+ new TrapezoidProfileCommand(
+ new TrapezoidProfile(
+ // Limit the max acceleration and velocity
+ new TrapezoidProfile.Constraints(kMaxSpeedMetersPerSecond,
+ kMaxAccelerationMetersPerSecondSquared),
+ // End at desired position in meters; implicitly starts at 0
+ new TrapezoidProfile.State(3, 0)),
+ // Pipe the profile state to the drive
+ setpointState -> m_robotDrive.setDriveStates(
+ setpointState,
+ setpointState),
+ // Require the drive
+ m_robotDrive)
+ .beforeStarting(m_robotDrive::resetEncoders)
+ .withTimeout(10));
+
+ // Drive at half speed when the bumper is held
+ new JoystickButton(m_driverController, Button.kBumperRight.value)
+ .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
+ .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+ }
+
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ return new InstantCommand();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
new file mode 100644
index 0000000..f89b36a
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.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.examples.drivedistanceoffboard.commands;
+
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
+
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxSpeedMetersPerSecond;
+
+/**
+ * Drives a set distance using a motion profile.
+ */
+public class DriveDistanceProfiled extends TrapezoidProfileCommand {
+ /**
+ * Creates a new DriveDistanceProfiled command.
+ *
+ * @param meters The distance to drive.
+ * @param drive The drive subsystem to use.
+ */
+ public DriveDistanceProfiled(double meters, DriveSubsystem drive) {
+ super(
+ new TrapezoidProfile(
+ // Limit the max acceleration and velocity
+ new TrapezoidProfile.Constraints(kMaxSpeedMetersPerSecond,
+ kMaxAccelerationMetersPerSecondSquared),
+ // End at desired position in meters; implicitly starts at 0
+ new TrapezoidProfile.State(meters, 0)),
+ // Pipe the profile state to the drive
+ setpointState -> drive.setDriveStates(setpointState, setpointState),
+ // Require the drive
+ drive);
+ // Reset drive encoders since we're starting at 0
+ drive.resetEncoders();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000..c8b98f7
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.drivedistanceoffboard.subsystems;
+
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.ExampleSmartMotorController;
+
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kLeftMotor1Port;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kLeftMotor2Port;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kRightMotor1Port;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kRightMotor2Port;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kaVoltSecondsSquaredPerMeter;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kp;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.ksVolts;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kvVoltSecondsPerMeter;
+
+public class DriveSubsystem extends SubsystemBase {
+ // The motors on the left side of the drive.
+ private final ExampleSmartMotorController m_leftMaster =
+ new ExampleSmartMotorController(kLeftMotor1Port);
+
+ private final ExampleSmartMotorController m_leftSlave =
+ new ExampleSmartMotorController(kLeftMotor2Port);
+
+ private final ExampleSmartMotorController m_rightMaster =
+ new ExampleSmartMotorController(kRightMotor1Port);
+
+ private final ExampleSmartMotorController m_rightSlave =
+ new ExampleSmartMotorController(kRightMotor2Port);
+
+ private final SimpleMotorFeedforward m_feedforward =
+ new SimpleMotorFeedforward(ksVolts,
+ kvVoltSecondsPerMeter,
+ kaVoltSecondsSquaredPerMeter);
+
+ // The robot's drive
+ private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMaster, m_rightMaster);
+
+ /**
+ * Creates a new DriveSubsystem.
+ */
+ public DriveSubsystem() {
+ m_leftSlave.follow(m_leftMaster);
+ m_rightSlave.follow(m_rightMaster);
+
+ m_leftMaster.setPID(kp, 0, 0);
+ m_rightMaster.setPID(kp, 0, 0);
+ }
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ public void arcadeDrive(double fwd, double rot) {
+ m_drive.arcadeDrive(fwd, rot);
+ }
+
+ /**
+ * Attempts to follow the given drive states using offboard PID.
+ *
+ * @param left The left wheel state.
+ * @param right The right wheel state.
+ */
+ public void setDriveStates(TrapezoidProfile.State left, TrapezoidProfile.State right) {
+ m_leftMaster.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition,
+ left.position,
+ m_feedforward.calculate(left.velocity));
+ m_rightMaster.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition,
+ right.position,
+ m_feedforward.calculate(right.velocity));
+ }
+
+ /**
+ * Returns the left encoder distance.
+ *
+ * @return the left encoder distance
+ */
+ public double getLeftEncoderDistance() {
+ return m_leftMaster.getEncoderDistance();
+ }
+
+ /**
+ * Returns the right encoder distance.
+ *
+ * @return the right encoder distance
+ */
+ public double getRightEncoderDistance() {
+ return m_rightMaster.getEncoderDistance();
+ }
+
+ /**
+ * Resets the drive encoders.
+ */
+ public void resetEncoders() {
+ m_leftMaster.resetEncoder();
+ m_rightMaster.resetEncoder();
+ }
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_drive.setMaxOutput(maxOutput);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Main.java
similarity index 88%
rename from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
rename to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Main.java
index d7f6e26..a051005 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Main.java
@@ -1,29 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* 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.templates.iterative;
-
-import edu.wpi.first.wpilibj.RobotBase;
-
-/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
- */
-public final class Main {
- private Main() {
- }
-
- /**
- * Main initialization function. Do not perform any initialization here.
- *
- * <p>If you change your main robot class, change the parameter type.
- */
- public static void main(String... args) {
- RobotBase.startRobot(Robot::new);
- }
-}
+/*----------------------------------------------------------------------------*/
+/* 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.examples.dutycycleencoder;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+ private Main() {
+ }
+
+ /**
+ * Main initialization function. Do not perform any initialization here.
+ *
+ * <p>If you change your main robot class, change the parameter type.
+ */
+ public static void main(String... args) {
+ RobotBase.startRobot(Robot::new);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java
new file mode 100644
index 0000000..5755f72
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.dutycycleencoder;
+
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+@SuppressWarnings({"PMD.SingularField"})
+public class Robot extends TimedRobot {
+ private DutyCycleEncoder m_dutyCycleEncoder;
+
+ @Override
+ public void robotInit() {
+ m_dutyCycleEncoder = new DutyCycleEncoder(0);
+
+ // Set to 0.5 units per rotation
+ m_dutyCycleEncoder.setDistancePerRotation(0.5);
+ }
+
+ @Override
+ public void robotPeriodic() {
+ // Connected can be checked, and uses the frequency of the encoder
+ boolean connected = m_dutyCycleEncoder.isConnected();
+
+ // Duty Cycle Frequency in Hz
+ int frequency = m_dutyCycleEncoder.getFrequency();
+
+ // Output of encoder
+ double output = m_dutyCycleEncoder.get();
+
+ // Output scaled by DistancePerPulse
+ double distance = m_dutyCycleEncoder.getDistance();
+
+ SmartDashboard.putBoolean("Connected", connected);
+ SmartDashboard.putNumber("Frequency", frequency);
+ SmartDashboard.putNumber("Output", output);
+ SmartDashboard.putNumber("Distance", distance);
+ }
+
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Main.java
similarity index 88%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Main.java
index d7f6e26..a7cbfc0 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Main.java
@@ -1,29 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* 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.templates.iterative;
-
-import edu.wpi.first.wpilibj.RobotBase;
-
-/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
- */
-public final class Main {
- private Main() {
- }
-
- /**
- * Main initialization function. Do not perform any initialization here.
- *
- * <p>If you change your main robot class, change the parameter type.
- */
- public static void main(String... args) {
- RobotBase.startRobot(Robot::new);
- }
-}
+/*----------------------------------------------------------------------------*/
+/* 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.examples.dutycycleinput;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+ private Main() {
+ }
+
+ /**
+ * Main initialization function. Do not perform any initialization here.
+ *
+ * <p>If you change your main robot class, change the parameter type.
+ */
+ public static void main(String... args) {
+ RobotBase.startRobot(Robot::new);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Robot.java
new file mode 100644
index 0000000..657dc9b
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Robot.java
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.dutycycleinput;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DutyCycle;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+@SuppressWarnings({"PMD.SingularField"})
+public class Robot extends TimedRobot {
+ private DigitalInput m_input;
+ private DutyCycle m_dutyCycle;
+
+ @Override
+ public void robotInit() {
+ m_input = new DigitalInput(0);
+ m_dutyCycle = new DutyCycle(m_input);
+ }
+
+ @Override
+ public void robotPeriodic() {
+ // Duty Cycle Frequency in Hz
+ int frequency = m_dutyCycle.getFrequency();
+
+ // Output of duty cycle, ranging from 0 to 1
+ // 1 is fully on, 0 is fully off
+ double output = m_dutyCycle.getOutput();
+
+ SmartDashboard.putNumber("Frequency", frequency);
+ SmartDashboard.putNumber("Duty Cycle", output);
+ }
+
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
index a48909f..5d3d546 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
@@ -9,7 +9,8 @@
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
@@ -19,7 +20,7 @@
private final Joystick m_joystick = new Joystick(1);
private final Encoder m_encoder = new Encoder(1, 2);
- private final Spark m_motor = new Spark(1);
+ private final SpeedController m_motor = new PWMVictorSPX(1);
// Create a PID controller whose setpoint's change is subject to maximum
// velocity and acceleration constraints.
@@ -30,7 +31,7 @@
@Override
public void robotInit() {
- m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);
+ m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
}
@Override
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java
new file mode 100644
index 0000000..d6a84ef
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.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.examples.elevatortrapezoidprofile;
+
+import edu.wpi.first.wpilibj.SpeedController;
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor controller.
+ *
+ * <p>Has no actual functionality.
+ */
+public class ExampleSmartMotorController implements SpeedController {
+ public enum PIDMode {
+ kPosition,
+ kVelocity,
+ kMovementWitchcraft
+ }
+
+ /**
+ * Creates a new ExampleSmartMotorController.
+ *
+ * @param port The port for the controller.
+ */
+ @SuppressWarnings("PMD.UnusedFormalParameter")
+ public ExampleSmartMotorController(int port) {
+ }
+
+ /**
+ * Example method for setting the PID gains of the smart controller.
+ *
+ * @param kp The proportional gain.
+ * @param ki The integral gain.
+ * @param kd The derivative gain.
+ */
+ public void setPID(double kp, double ki, double kd) {
+ }
+
+ /**
+ * Example method for setting the setpoint of the smart controller in PID mode.
+ *
+ * @param mode The mode of the PID controller.
+ * @param setpoint The controller setpoint.
+ * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+ */
+ public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {
+ }
+
+ /**
+ * Places this motor controller in follower mode.
+ *
+ * @param master The master to follow.
+ */
+ public void follow(ExampleSmartMotorController master) {
+ }
+
+ /**
+ * Returns the encoder distance.
+ *
+ * @return The current encoder distance.
+ */
+ public double getEncoderDistance() {
+ return 0;
+ }
+
+ /**
+ * Returns the encoder rate.
+ *
+ * @return The current encoder rate.
+ */
+ public double getEncoderRate() {
+ return 0;
+ }
+
+ /**
+ * Resets the encoder to zero distance.
+ */
+ public void resetEncoder() {
+ }
+
+ @Override
+ public void set(double speed) {
+ }
+
+ @Override
+ public double get() {
+ return 0;
+ }
+
+ @Override
+ public void setInverted(boolean isInverted) {
+
+ }
+
+ @Override
+ public boolean getInverted() {
+ return false;
+ }
+
+ @Override
+ public void disable() {
+ }
+
+ @Override
+ public void stopMotor() {
+ }
+
+ @Override
+ public void pidWrite(double output) {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
index fddd417..918d3d3 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
@@ -7,20 +7,18 @@
package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
-import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
public class Robot extends TimedRobot {
private static double kDt = 0.02;
private final Joystick m_joystick = new Joystick(1);
- private final Encoder m_encoder = new Encoder(1, 2);
- private final Spark m_motor = new Spark(1);
- private final PIDController m_controller = new PIDController(1.3, 0.0, 0.7, kDt);
+ private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(1);
+ // Note: These gains are fake, and will have to be tuned for your robot.
+ private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1.5);
private final TrapezoidProfile.Constraints m_constraints =
new TrapezoidProfile.Constraints(1.75, 0.75);
@@ -29,7 +27,8 @@
@Override
public void robotInit() {
- m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);
+ // Note: These gains are fake, and will have to be tuned for your robot.
+ m_motor.setPID(1.3, 0.0, 0.7);
}
@Override
@@ -49,10 +48,8 @@
// toward the goal while obeying the constraints.
m_setpoint = profile.calculate(kDt);
- double output = m_controller.calculate(m_encoder.getDistance(),
- m_setpoint.position);
-
- // Run controller with profiled setpoint and update motor output
- m_motor.set(output);
+ // Send setpoint to offboard controller PID
+ m_motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, m_setpoint.position,
+ m_feedforward.calculate(m_setpoint.velocity) / 12.0);
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java
index 4bb26ea..ef34fe5 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java
@@ -54,7 +54,7 @@
* attached to a 3 inch diameter (1.5inch radius) wheel,
* and that we want to measure distance in inches.
*/
- m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);
+ m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
/*
* Defines the lowest rate at which the encoder will
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
index 39840f7..6dd4178 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
@@ -7,7 +7,8 @@
],
"foldername": "gettingstarted",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Tank Drive",
@@ -20,7 +21,8 @@
],
"foldername": "tankdrive",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Arcade Drive",
@@ -30,7 +32,8 @@
],
"foldername": "arcadedrive",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Mecanum Drive",
@@ -43,7 +46,8 @@
],
"foldername": "mecanumdrive",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "PDP CAN Monitoring",
@@ -55,7 +59,8 @@
],
"foldername": "canpdp",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Solenoids",
@@ -68,7 +73,8 @@
],
"foldername": "solenoid",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Encoder",
@@ -80,7 +86,8 @@
],
"foldername": "encoder",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Relay",
@@ -92,7 +99,8 @@
],
"foldername": "relay",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Ultrasonic",
@@ -104,7 +112,8 @@
],
"foldername": "ultrasonic",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Ultrasonic PID",
@@ -116,7 +125,8 @@
],
"foldername": "ultrasonicpid",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Potentiometer PID",
@@ -129,7 +139,8 @@
],
"foldername": "potentiometerpid",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Elevator with trapezoid profiled PID",
@@ -142,7 +153,8 @@
],
"foldername": "elevatortrapezoidprofile",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Elevator with profiled PID controller",
@@ -155,7 +167,8 @@
],
"foldername": "elevatorprofiledpid",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Gyro",
@@ -168,7 +181,8 @@
],
"foldername": "gyro",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Gyro Mecanum",
@@ -181,7 +195,8 @@
],
"foldername": "gyromecanum",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "HID Rumble",
@@ -191,7 +206,8 @@
],
"foldername": "hidrumble",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Motor Controller",
@@ -203,7 +219,8 @@
],
"foldername": "motorcontrol",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Motor Control With Encoder",
@@ -217,8 +234,9 @@
"Complete List"
],
"foldername": "motorcontrolencoder",
- "gradlebase": "java"
- ,"mainclass": "Main"
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "GearsBot",
@@ -228,7 +246,8 @@
],
"foldername": "gearsbot",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "PacGoat",
@@ -238,7 +257,8 @@
],
"foldername": "pacgoat",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Simple Vision",
@@ -249,7 +269,8 @@
],
"foldername": "quickvision",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Intermediate Vision",
@@ -260,7 +281,8 @@
],
"foldername": "intermediatevision",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Axis Camera Sample",
@@ -270,7 +292,8 @@
],
"foldername": "axiscamera",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Shuffleboard Sample",
@@ -281,7 +304,8 @@
],
"foldername": "shuffleboard",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "'Traditional' Hatchbot",
@@ -292,7 +316,8 @@
],
"foldername": "hatchbottraditional",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "'Inlined' Hatchbot",
@@ -304,7 +329,8 @@
],
"foldername": "hatchbotinlined",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "Select Command Example",
@@ -314,7 +340,8 @@
],
"foldername": "selectcommand",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "Scheduler Event Logging",
@@ -325,7 +352,8 @@
],
"foldername": "schedulereventlogging",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "Frisbeebot",
@@ -336,7 +364,8 @@
],
"foldername": "frisbeebot",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "Gyro Drive Commands",
@@ -348,7 +377,8 @@
],
"foldername": "gyrodrivecommands",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "SwerveBot",
@@ -358,7 +388,8 @@
],
"foldername": "swervebot",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "MecanumBot",
@@ -368,7 +399,8 @@
],
"foldername": "mecanumbot",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "DifferentialDriveBot",
@@ -378,10 +410,11 @@
],
"foldername": "differentialdrivebot",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
- "name:": "RamseteCommand",
+ "name": "RamseteCommand",
"description": "An example command-based robot demonstrating the use of a RamseteCommand to follow a pregenerated trajectory.",
"tags": [
"RamseteCommand",
@@ -392,6 +425,131 @@
],
"foldername": "ramsetecommand",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "Arcade Drive Xbox Controller",
+ "description": "Demonstrates the use of the DifferentialDrive class to drive a robot with Arcade Drive and an Xbox Controller",
+ "tags": [
+ "Getting Started with Java"
+ ],
+ "foldername": "arcadedrivexboxcontroller",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 1
+ },
+ {
+ "name": "Tank Drive Xbox Controller",
+ "description": "Demonstrates the use of the DifferentialDrive class to drive a robot with Tank Drive and an Xbox Controller",
+ "tags": [
+ "Getting Started with Java"
+ ],
+ "foldername": "tankdrivexboxcontroller",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 1
+ },
+ {
+ "name": "Duty Cycle Encoder",
+ "description": "Demonstrates the use of the Duty Cycle Encoder class",
+ "tags": [
+ "Getting Started with Java"
+ ],
+ "foldername": "dutycycleencoder",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "Duty Cycle Input",
+ "description": "Demonstrates the use of the Duty Cycle class",
+ "tags": [
+ "Getting Started with Java"
+ ],
+ "foldername": "dutycycleinput",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "Addressable LED",
+ "description": "Demonstrates the use of the Addressable LED class",
+ "tags": [
+ "Getting Started with Java"
+ ],
+ "foldername": "addressableled",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "ArmBot",
+ "description": "An example command-based robot demonstrating the use of a ProfiledPIDSubsystem to control an arm.",
+ "tags": [
+ "ArmBot",
+ "PID",
+ "Motion Profile"
+ ],
+ "foldername": "armbot",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "ArmBotOffboard",
+ "description": "An example command-based robot demonstrating the use of a TrapezoidProfileSubsystem to control an arm with an offboard PID.",
+ "tags": [
+ "ArmBotOffboard",
+ "PID",
+ "Motion Profile"
+ ],
+ "foldername": "armbotoffboard",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "DriveDistanceOffboard",
+ "description": "An example command-based robot demonstrating the use of a TrapezoidProfileCommand to drive a robot a set distance with offboard PID on the drive.",
+ "tags": [
+ "DriveDistance",
+ "PID",
+ "Motion Profile"
+ ],
+ "foldername": "drivedistanceoffboard",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "MecanumControllerCommand",
+ "description": "An example command-based robot demonstrating the use of a MecanumControllerCommand to follow a pregenerated trajectory.",
+ "tags": [
+ "MecanumControllerCommand",
+ "Mecanum",
+ "PID",
+ "Trajectory",
+ "Path following"
+ ],
+ "foldername": "mecanumcontrollercommand",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "SwerveControllerCommand",
+ "description": "An example command-based robot demonstrating the use of a SwerveControllerCommand to follow a pregenerated trajectory.",
+ "tags": [
+ "SwerveControllerCommand",
+ "Swerve",
+ "PID",
+ "Trajectory",
+ "Path following"
+ ],
+ "foldername": "swervecontrollercommand",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
}
]
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
index 384fb98..c8c422c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
@@ -40,7 +40,7 @@
public static final int kEncoderCPR = 1024;
public static final double kEncoderDistancePerPulse =
// Distance units will be rotations
- 1. / (double) kEncoderCPR;
+ 1.0 / (double) kEncoderCPR;
public static final int kShooterMotorPort = 4;
public static final int kFeederMotorPort = 5;
@@ -49,18 +49,19 @@
public static final double kShooterTargetRPS = 4000;
public static final double kShooterToleranceRPS = 50;
+ // These are not real PID gains, and will have to be tuned for your specific robot.
public static final double kP = 1;
public static final double kI = 0;
public static final double kD = 0;
// On a real robot the feedforward constants should be empirically determined; these are
// reasonable guesses.
- public static final double kSFractional = .05;
- public static final double kVFractional =
- // Should have value 1 at free speed...
- 1. / kShooterFreeRPS;
+ public static final double kSVolts = 0.05;
+ public static final double kVVoltSecondsPerRotation =
+ // Should have value 12V at free speed...
+ 12.0 / kShooterFreeRPS;
- public static final double kFeederSpeed = .5;
+ public static final double kFeederSpeed = 0.5;
}
public static final class AutoConstants {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
index ea6377a..69008ae 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
@@ -103,7 +103,7 @@
// Drive at half speed when the bumper is held
new JoystickButton(m_driverController, Button.kBumperRight.value)
- .whenPressed(() -> m_robotDrive.setMaxOutput(.5))
+ .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
index c9d8729..192fa30 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
@@ -78,7 +78,7 @@
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
- return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
index 5ebfb04..c870114 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
@@ -10,6 +10,7 @@
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kD;
@@ -20,17 +21,19 @@
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kFeederSpeed;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kI;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kP;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kSFractional;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kSVolts;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterMotorPort;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterTargetRPS;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterToleranceRPS;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kVFractional;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kVVoltSecondsPerRotation;
public class ShooterSubsystem extends PIDSubsystem {
private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(kShooterMotorPort);
private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(kFeederMotorPort);
private final Encoder m_shooterEncoder =
new Encoder(kEncoderPorts[0], kEncoderPorts[1], kEncoderReversed);
+ private final SimpleMotorFeedforward m_shooterFeedforward
+ = new SimpleMotorFeedforward(kSVolts, kVVoltSecondsPerRotation);
/**
* The shooter subsystem for the robot.
@@ -39,17 +42,12 @@
super(new PIDController(kP, kI, kD));
getController().setTolerance(kShooterToleranceRPS);
m_shooterEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ setSetpoint(kShooterTargetRPS);
}
@Override
- public void useOutput(double output) {
- // Use a feedforward of the form kS + kV * velocity
- m_shooterMotor.set(output + kSFractional + kVFractional * kShooterTargetRPS);
- }
-
- @Override
- public double getSetpoint() {
- return kShooterTargetRPS;
+ public void useOutput(double output, double setpoint) {
+ m_shooterMotor.setVoltage(output + m_shooterFeedforward.calculate(setpoint));
}
@Override
@@ -68,12 +66,4 @@
public void stopFeeder() {
m_feederMotor.set(0);
}
-
- @Override
- public void disable() {
- super.disable();
- // Turn off motor when we disable, since useOutput(0) doesn't stop the motor due to our
- // feedforward
- m_shooterMotor.set(0);
- }
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
index fffd478..4f5c77f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
@@ -22,7 +22,6 @@
public class Elevator extends PIDSubsystem {
private final Victor m_motor;
private final AnalogPotentiometer m_pot;
- private double m_setpoint;
private static final double kP_real = 4;
private static final double kI_real = 0.07;
@@ -73,26 +72,7 @@
* Use the motor as the PID output. This method is automatically called by the subsystem.
*/
@Override
- public void useOutput(double output) {
+ public void useOutput(double output, double setpoint) {
m_motor.set(output);
}
-
- /**
- * Returns the setpoint used by the PIDController.
- *
- * @return The setpoint for the PIDController.
- */
- @Override
- public double getSetpoint() {
- return m_setpoint;
- }
-
- /**
- * Sets the setpoint used by the PIDController.
- *
- * @param setpoint The setpoint for the PIDController.
- */
- public void setSetpoint(double setpoint) {
- m_setpoint = setpoint;
- }
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
index ae65bed..dbd952e 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
@@ -21,7 +21,6 @@
public class Wrist extends PIDSubsystem {
private final Victor m_motor;
private final AnalogPotentiometer m_pot;
- private double m_setpoint;
private static final double kP_real = 1;
private static final double kP_simulation = 0.05;
@@ -70,26 +69,7 @@
* Use the motor as the PID output. This method is automatically called by the subsystem.
*/
@Override
- public void useOutput(double output) {
+ public void useOutput(double output, double setpoint) {
m_motor.set(output);
}
-
- /**
- * Returns the setpoint used by the PIDController.
- *
- * @return The setpoint for the PIDController.
- */
- @Override
- public double getSetpoint() {
- return m_setpoint;
- }
-
- /**
- * Sets the setpoint used by the PIDController.
- *
- * @param setpoint The setpoint for the PIDController.
- */
- public void setSetpoint(double setpoint) {
- m_setpoint = setpoint;
- }
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java
index 50afb7b..473d6e1 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java
@@ -36,13 +36,16 @@
public static final boolean kGyroReversed = false;
public static final double kStabilizationP = 1;
- public static final double kStabilizationI = .5;
+ public static final double kStabilizationI = 0.5;
public static final double kStabilizationD = 0;
public static final double kTurnP = 1;
public static final double kTurnI = 0;
public static final double kTurnD = 0;
+ public static final double kMaxTurnRateDegPerS = 100;
+ public static final double kMaxTurnAccelerationDegPerSSquared = 300;
+
public static final double kTurnToleranceDeg = 5;
public static final double kTurnRateToleranceDegPerS = 10; // degrees per second
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
index 5a6d632..44b09b8 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
@@ -17,6 +17,7 @@
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngle;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngleProfiled;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
import static edu.wpi.first.wpilibj.XboxController.Button;
@@ -65,7 +66,7 @@
private void configureButtonBindings() {
// Drive at half speed when the right bumper is held
new JoystickButton(m_driverController, Button.kBumperRight.value)
- .whenPressed(() -> m_robotDrive.setMaxOutput(.5))
+ .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
// Stabilize robot to drive straight with gyro when left bumper is held
@@ -85,6 +86,10 @@
// Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout
new JoystickButton(m_driverController, Button.kX.value)
.whenPressed(new TurnToAngle(90, m_robotDrive).withTimeout(5));
+
+ // Turn to -90 degrees with a profile when the 'A' button is pressed, with a 5 second timeout
+ new JoystickButton(m_driverController, Button.kA.value)
+ .whenPressed(new TurnToAngleProfiled(-90, m_robotDrive).withTimeout(5));
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java
new file mode 100644
index 0000000..8fb4320
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.gyrodrivecommands.commands;
+
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
+
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kMaxTurnAccelerationDegPerSSquared;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kMaxTurnRateDegPerS;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnD;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnI;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnP;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnRateToleranceDegPerS;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnToleranceDeg;
+
+/**
+ * A command that will turn the robot to the specified angle using a motion profile.
+ */
+public class TurnToAngleProfiled extends ProfiledPIDCommand {
+ /**
+ * Turns to robot to the specified angle using a motion profile.
+ *
+ * @param targetAngleDegrees The angle to turn to
+ * @param drive The drive subsystem to use
+ */
+ public TurnToAngleProfiled(double targetAngleDegrees, DriveSubsystem drive) {
+ super(
+ new ProfiledPIDController(kTurnP, kTurnI, kTurnD,
+ new TrapezoidProfile.Constraints(
+ kMaxTurnRateDegPerS,
+ kMaxTurnAccelerationDegPerSSquared
+ )),
+ // Close loop on heading
+ drive::getHeading,
+ // Set reference to target
+ targetAngleDegrees,
+ // Pipe output to turn robot
+ (output, setpoint) -> drive.arcadeDrive(0, output),
+ // Require the drive
+ drive);
+
+ // Set the controller to be continuous (because it is an angle controller)
+ getController().enableContinuousInput(-180, 180);
+ // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
+ // setpoint before it is considered as having reached the reference
+ getController().setTolerance(kTurnToleranceDeg, kTurnRateToleranceDegPerS);
+ }
+
+ @Override
+ public boolean isFinished() {
+ // End when the controller is at the reference.
+ return getController().atGoal();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
index e91b8b3..a4f001e 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
@@ -84,7 +84,7 @@
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
- return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
@@ -127,7 +127,7 @@
* @return the robot's heading in degrees, from 180 to 180
*/
public double getHeading() {
- return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1. : 1.);
+ return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
}
/**
@@ -136,6 +136,6 @@
* @return The turn rate of the robot, in degrees per second
*/
public double getTurnRate() {
- return m_gyro.getRate() * (kGyroReversed ? -1. : 1.);
+ return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0);
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java
index 27cb872..8029888 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java
@@ -42,7 +42,7 @@
public static final class AutoConstants {
public static final double kAutoDriveDistanceInches = 60;
public static final double kAutoBackupDistanceInches = 20;
- public static final double kAutoDriveSpeed = .5;
+ public static final double kAutoDriveSpeed = 0.5;
}
public static final class OIConstants {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
index 9310d39..a5c7129 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
@@ -105,7 +105,7 @@
.whenPressed(new InstantCommand(m_hatchSubsystem::releaseHatch, m_hatchSubsystem));
// While holding the shoulder button, drive at half speed
new JoystickButton(m_driverController, Button.kBumperRight.value)
- .whenPressed(() -> m_robotDrive.setMaxOutput(.5))
+ .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
index 233cb85..5361e87 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
@@ -78,7 +78,7 @@
* @return the average of the TWO encoder readings
*/
public double getAverageEncoderDistance() {
- return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java
index 1e9e060..2674b50 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java
@@ -42,7 +42,7 @@
public static final class AutoConstants {
public static final double kAutoDriveDistanceInches = 60;
public static final double kAutoBackupDistanceInches = 20;
- public static final double kAutoDriveSpeed = .5;
+ public static final double kAutoDriveSpeed = 0.5;
}
public static final class OIConstants {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java
index 396d40d..bd6c093 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java
@@ -20,7 +20,7 @@
@Override
public void initialize() {
- m_drive.setMaxOutput(.5);
+ m_drive.setMaxOutput(0.5);
}
@Override
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
index 6cadc1f..a068b56 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
@@ -78,7 +78,7 @@
* @return the average of the TWO encoder readings
*/
public double getAverageEncoderDistance() {
- return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
index 29a6f49..6220807 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
@@ -9,7 +9,8 @@
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
@@ -26,15 +27,15 @@
public static final double kMaxSpeed = 3.0; // 3 meters per second
public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
- private final Spark m_frontLeftMotor = new Spark(1);
- private final Spark m_frontRightMotor = new Spark(2);
- private final Spark m_backLeftMotor = new Spark(3);
- private final Spark m_backRightMotor = new Spark(4);
+ private final SpeedController m_frontLeftMotor = new PWMVictorSPX(1);
+ private final SpeedController m_frontRightMotor = new PWMVictorSPX(2);
+ private final SpeedController m_backLeftMotor = new PWMVictorSPX(3);
+ private final SpeedController m_backRightMotor = new PWMVictorSPX(4);
private final Encoder m_frontLeftEncoder = new Encoder(0, 1);
- private final Encoder m_frontRightEncoder = new Encoder(0, 1);
- private final Encoder m_backLeftEncoder = new Encoder(0, 1);
- private final Encoder m_backRightEncoder = new Encoder(0, 1);
+ private final Encoder m_frontRightEncoder = new Encoder(2, 3);
+ private final Encoder m_backLeftEncoder = new Encoder(4, 5);
+ private final Encoder m_backRightEncoder = new Encoder(6, 7);
private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
@@ -52,7 +53,8 @@
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
);
- private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics);
+ private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
+ getAngle());
/**
* Constructs a MecanumDrive and resets the gyro.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java
new file mode 100644
index 0000000..88f904b
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java
@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.mecanumcontrollercommand;
+
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kFrontLeftMotorPort = 0;
+ public static final int kRearLeftMotorPort = 1;
+ public static final int kFrontRightMotorPort = 2;
+ public static final int kRearRightMotorPort = 3;
+
+ public static final int[] kFrontLeftEncoderPorts = new int[]{0, 1};
+ public static final int[] kRearLeftEncoderPorts = new int[]{2, 3};
+ public static final int[] kFrontRightEncoderPorts = new int[]{4, 5};
+ public static final int[] kRearRightEncoderPorts = new int[]{5, 6};
+
+ public static final boolean kFrontLeftEncoderReversed = false;
+ public static final boolean kRearLeftEncoderReversed = true;
+ public static final boolean kFrontRightEncoderReversed = false;
+ public static final boolean kRearRightEncoderReversed = true;
+
+ public static final double kTrackWidth = 0.5;
+ // Distance between centers of right and left wheels on robot
+ public static final double kWheelBase = 0.7;
+ // Distance between centers of front and back wheels on robot
+
+ public static final MecanumDriveKinematics kDriveKinematics =
+ new MecanumDriveKinematics(
+ new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+
+ public static final int kEncoderCPR = 1024;
+ public static final double kWheelDiameterMeters = 0.15;
+ public static final double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
+
+ public static final boolean kGyroReversed = false;
+
+ // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+ // These characterization values MUST be determined either experimentally or theoretically
+ // for *your* robot's drive.
+ // The RobotPy Characterization Toolsuite provides a convenient tool for obtaining these
+ // values for your robot.
+ public static final SimpleMotorFeedforward kFeedforward =
+ new SimpleMotorFeedforward(1, 0.8, 0.15);
+
+ // Example value only - as above, this must be tuned for your drive!
+ public static final double kPFrontLeftVel = 0.5;
+ public static final double kPRearLeftVel = 0.5;
+ public static final double kPFrontRightVel = 0.5;
+ public static final double kPRearRightVel = 0.5;
+
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 1;
+
+ }
+
+ public static final class AutoConstants {
+ public static final double kMaxSpeedMetersPerSecond = 3;
+ public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+ public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
+ public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
+
+ public static final double kPXController = 0.5;
+ public static final double kPYController = 0.5;
+ public static final double kPThetaController = 0.5;
+
+ //Constraint for the motion profilied robot angle controller
+ public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
+ new TrapezoidProfile.Constraints(kMaxAngularSpeedRadiansPerSecond,
+ kMaxAngularSpeedRadiansPerSecondSquared);
+
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java
similarity index 75%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java
index d7f6e26..9670d3d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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.templates.iterative;
+package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java
new file mode 100644
index 0000000..6f2ccbf
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.mecanumcontrollercommand;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
new file mode 100644
index 0000000..54707e3
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
@@ -0,0 +1,148 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.mecanumcontrollercommand;
+
+import java.util.List;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.XboxController.Button;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+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.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.MecanumControllerCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kPThetaController;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kPXController;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kPYController;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kThetaControllerConstraints;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kDriveKinematics;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFeedforward;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPFrontLeftVel;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPFrontRightVel;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPRearLeftVel;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPRearRightVel;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.OIConstants.kDriverControllerPort;
+
+/*
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+@SuppressWarnings("PMD.ExcessiveImports")
+public class RobotContainer {
+ // The robot's subsystems
+ private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+
+ // The driver's controller
+ XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+
+ // Configure default commands
+ // Set the default drive command to split-stick arcade drive
+ m_robotDrive.setDefaultCommand(
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ new RunCommand(() -> m_robotDrive.drive(
+ m_driverController.getY(GenericHID.Hand.kLeft),
+ m_driverController.getX(GenericHID.Hand.kRight),
+ m_driverController.getX(GenericHID.Hand.kLeft), false)));
+
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
+ * {@link JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ // Drive at half speed when the right bumper is held
+ new JoystickButton(m_driverController, Button.kBumperRight.value)
+ .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
+ .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+
+ }
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ // Create config for trajectory
+ TrajectoryConfig config =
+ new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared)
+ // Add kinematics to ensure max speed is actually obeyed
+ .setKinematics(kDriveKinematics);
+
+ // An example trajectory to follow. All units in meters.
+ Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
+ // Start at the origin facing the +X direction
+ new Pose2d(0, 0, new Rotation2d(0)),
+ // Pass through these two interior waypoints, making an 's' curve path
+ List.of(
+ new Translation2d(1, 1),
+ new Translation2d(2, - 1)
+ ),
+ // End 3 meters straight ahead of where we started, facing forward
+ new Pose2d(3, 0, new Rotation2d(0)),
+ config
+ );
+
+ MecanumControllerCommand mecanumControllerCommand = new MecanumControllerCommand(
+ exampleTrajectory,
+ m_robotDrive::getPose,
+
+ kFeedforward,
+ kDriveKinematics,
+
+ //Position contollers
+ new PIDController(kPXController, 0, 0),
+ new PIDController(kPYController, 0, 0),
+ new ProfiledPIDController(kPThetaController, 0, 0, kThetaControllerConstraints),
+
+ //Needed for normalizing wheel speeds
+ kMaxSpeedMetersPerSecond,
+
+ //Velocity PID's
+ new PIDController(kPFrontLeftVel, 0, 0),
+ new PIDController(kPRearLeftVel, 0, 0),
+ new PIDController(kPFrontRightVel, 0, 0),
+ new PIDController(kPRearRightVel, 0, 0),
+
+ m_robotDrive::getCurrentWheelSpeeds,
+
+ m_robotDrive::setDriveSpeedControllersVolts, //Consumer for the output motor voltages
+
+ m_robotDrive
+ );
+
+ // Run path following command, then stop at the end.
+ return mecanumControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000..b68f770
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
@@ -0,0 +1,253 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.mecanumcontrollercommand.subsystems;
+
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.drive.MecanumDrive;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kDriveKinematics;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontLeftMotorPort;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontRightMotorPort;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kGyroReversed;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearLeftMotorPort;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearRightMotorPort;
+
+public class DriveSubsystem extends SubsystemBase {
+ private final PWMVictorSPX m_frontLeft = new PWMVictorSPX(kFrontLeftMotorPort);
+ private final PWMVictorSPX m_rearLeft = new PWMVictorSPX(kRearLeftMotorPort);
+ private final PWMVictorSPX m_frontRight = new PWMVictorSPX(kFrontRightMotorPort);
+ private final PWMVictorSPX m_rearRight = new PWMVictorSPX(kRearRightMotorPort);
+
+ private final MecanumDrive m_drive = new MecanumDrive(
+ m_frontLeft,
+ m_rearLeft,
+ m_frontRight,
+ m_rearRight);
+
+ // The front-left-side drive encoder
+ private final Encoder m_frontLeftEncoder =
+ new Encoder(kFrontLeftEncoderPorts[0], kFrontLeftEncoderPorts[1],
+ kFrontLeftEncoderReversed);
+
+ // The rear-left-side drive encoder
+ private final Encoder m_rearLeftEncoder =
+ new Encoder(kRearLeftEncoderPorts[0], kRearLeftEncoderPorts[1],
+ kRearLeftEncoderReversed);
+
+ // The front-right--side drive encoder
+ private final Encoder m_frontRightEncoder =
+ new Encoder(kFrontRightEncoderPorts[0], kFrontRightEncoderPorts[1],
+ kFrontRightEncoderReversed);
+
+ // The rear-right-side drive encoder
+ private final Encoder m_rearRightEncoder =
+ new Encoder(kRearRightEncoderPorts[0], kRearRightEncoderPorts[1],
+ kRearRightEncoderReversed);
+
+ // The gyro sensor
+ private final Gyro m_gyro = new ADXRS450_Gyro();
+
+ // Odometry class for tracking robot pose
+ MecanumDriveOdometry m_odometry =
+ new MecanumDriveOdometry(kDriveKinematics, getAngle());
+
+ /**
+ * Creates a new DriveSubsystem.
+ */
+ public DriveSubsystem() {
+ // Sets the distance per pulse for the encoders
+ m_frontLeftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_rearLeftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_frontRightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_rearRightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ }
+
+ /**
+ * Returns the angle of the robot as a Rotation2d.
+ *
+ * @return The angle of the robot.
+ */
+ public Rotation2d getAngle() {
+ // Negating the angle because WPILib gyros are CW positive.
+ return Rotation2d.fromDegrees(m_gyro.getAngle() * (kGyroReversed ? 1.0 : -1.0));
+ }
+
+ @Override
+ public void periodic() {
+ // Update the odometry in the periodic block
+ m_odometry.update(getAngle(),
+ new MecanumDriveWheelSpeeds(
+ m_frontLeftEncoder.getRate(),
+ m_rearLeftEncoder.getRate(),
+ m_frontRightEncoder.getRate(),
+ m_rearRightEncoder.getRate()));
+ }
+
+ /**
+ * Returns the currently-estimated pose of the robot.
+ *
+ * @return The pose.
+ */
+ public Pose2d getPose() {
+ return m_odometry.getPoseMeters();
+ }
+
+ /**
+ * Resets the odometry to the specified pose.
+ *
+ * @param pose The pose to which to set the odometry.
+ */
+ public void resetOdometry(Pose2d pose) {
+ m_odometry.resetPosition(pose, getAngle());
+ }
+
+ /**
+ * Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1] and the linear
+ * speeds have no effect on the angular speed.
+ *
+ * @param xSpeed Speed of the robot in the x direction (forward/backwards).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
+ * @param fieldRelative Whether the provided x and y speeds are relative to the field.
+ */
+ @SuppressWarnings("ParameterName")
+ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+ if ( fieldRelative ) {
+ m_drive.driveCartesian(ySpeed, xSpeed, rot, -m_gyro.getAngle());
+ } else {
+ m_drive.driveCartesian(ySpeed, xSpeed, rot);
+ }
+
+ }
+
+ /**
+ * Sets the front left drive SpeedController to a voltage.
+ */
+ public void setDriveSpeedControllersVolts(MecanumDriveMotorVoltages volts) {
+ m_frontLeft.setVoltage(volts.frontLeftVoltage);
+ m_rearLeft.setVoltage(volts.rearLeftVoltage);
+ m_frontRight.setVoltage(volts.frontRightVoltage);
+ m_rearRight.setVoltage(volts.rearRightVoltage);
+ }
+
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ public void resetEncoders() {
+ m_frontLeftEncoder.reset();
+ m_rearLeftEncoder.reset();
+ m_frontRightEncoder.reset();
+ m_rearRightEncoder.reset();
+ }
+
+ /**
+ * Gets the front left drive encoder.
+ *
+ * @return the front left drive encoder
+ */
+
+ public Encoder getFrontLeftEncoder() {
+ return m_frontLeftEncoder;
+ }
+
+ /**
+ * Gets the rear left drive encoder.
+ *
+ * @return the rear left drive encoder
+ */
+
+ public Encoder getRearLeftEncoder() {
+ return m_rearLeftEncoder;
+ }
+
+ /**
+ * Gets the front right drive encoder.
+ *
+ * @return the front right drive encoder
+ */
+
+ public Encoder getFrontRightEncoder() {
+ return m_frontRightEncoder;
+ }
+ /**
+ * Gets the rear right drive encoder.
+ *
+ * @return the rear right encoder
+ */
+
+ public Encoder getRearRightEncoder() {
+ return m_rearRightEncoder;
+ }
+
+ /**
+ * Gets the current wheel speeds.
+ *
+ * @return the current wheel speeds in a MecanumDriveWheelSpeeds object.
+ */
+
+ public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
+ return new MecanumDriveWheelSpeeds(m_frontLeftEncoder.getRate(),
+ m_rearLeftEncoder.getRate(),
+ m_frontRightEncoder.getRate(),
+ m_rearRightEncoder.getRate());
+ }
+
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_drive.setMaxOutput(maxOutput);
+ }
+
+ /**
+ * Zeroes the heading of the robot.
+ */
+ public void zeroHeading() {
+ m_gyro.reset();
+ }
+
+ /**
+ * Returns the heading of the robot.
+ *
+ * @return the robot's heading in degrees, from 180 to 180
+ */
+ public double getHeading() {
+ return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
+ }
+
+ /**
+ * Returns the turn rate of the robot.
+ *
+ * @return The turn rate of the robot, in degrees per second
+ */
+ public double getTurnRate() {
+ return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
index f7aecdd..7d47655 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
@@ -8,7 +8,6 @@
package edu.wpi.first.wpilibj.examples.ramsetecommand;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -30,12 +29,12 @@
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
- public static final double kTrackwidthMeters = .6;
+ public static final double kTrackwidthMeters = 0.6;
public static final DifferentialDriveKinematics kDriveKinematics =
new DifferentialDriveKinematics(kTrackwidthMeters);
public static final int kEncoderCPR = 1024;
- public static final double kWheelDiameterMeters = .15;
+ public static final double kWheelDiameterMeters = 0.15;
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
@@ -48,11 +47,11 @@
// The RobotPy Characterization Toolsuite provides a convenient tool for obtaining these
// values for your robot.
public static final double ksVolts = 1;
- public static final double kvVoltSecondsPerMeter = .8;
- public static final double kaVoltSecondsSquaredPerMeter = .15;
+ public static final double kvVoltSecondsPerMeter = 0.8;
+ public static final double kaVoltSecondsSquaredPerMeter = 0.15;
// Example value only - as above, this must be tuned for your drive!
- public static final double kPDriveVel = .5;
+ public static final double kPDriveVel = 0.5;
}
public static final class OIConstants {
@@ -63,12 +62,8 @@
public static final double kMaxSpeedMetersPerSecond = 3;
public static final double kMaxAccelerationMetersPerSecondSquared = 3;
- public static final DifferentialDriveKinematicsConstraint kAutoPathConstraints =
- new DifferentialDriveKinematicsConstraint(DriveConstants.kDriveKinematics,
- kMaxSpeedMetersPerSecond);
-
// Reasonable baseline values for a RAMSETE follower in units of meters and seconds
public static final double kRamseteB = 2;
- public static final double kRamseteZeta = .7;
+ public static final double kRamseteZeta = 0.7;
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
index f8a3a25..c575e29 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
@@ -13,12 +13,14 @@
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
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.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -78,7 +80,7 @@
private void configureButtonBindings() {
// Drive at half speed when the right bumper is held
new JoystickButton(m_driverController, Button.kBumperRight.value)
- .whenPressed(() -> m_robotDrive.setMaxOutput(.5))
+ .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
}
@@ -90,11 +92,23 @@
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
+
+ // Create a voltage constraint to ensure we don't accelerate too fast
+ var autoVoltageConstraint =
+ new DifferentialDriveVoltageConstraint(
+ new SimpleMotorFeedforward(ksVolts,
+ kvVoltSecondsPerMeter,
+ kaVoltSecondsSquaredPerMeter),
+ kDriveKinematics,
+ 10);
+
// Create config for trajectory
TrajectoryConfig config =
new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
- .setKinematics(kDriveKinematics);
+ .setKinematics(kDriveKinematics)
+ // Apply the voltage constraint
+ .addConstraint(autoVoltageConstraint);
// An example trajectory to follow. All units in meters.
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
@@ -115,20 +129,17 @@
exampleTrajectory,
m_robotDrive::getPose,
new RamseteController(kRamseteB, kRamseteZeta),
- ksVolts,
- kvVoltSecondsPerMeter,
- kaVoltSecondsSquaredPerMeter,
+ new SimpleMotorFeedforward(ksVolts, kvVoltSecondsPerMeter, kaVoltSecondsSquaredPerMeter),
kDriveKinematics,
- m_robotDrive.getLeftEncoder()::getRate,
- m_robotDrive.getRightEncoder()::getRate,
+ m_robotDrive::getWheelSpeeds,
new PIDController(kPDriveVel, 0, 0),
new PIDController(kPDriveVel, 0, 0),
- // RamseteCommand passes volts to the callback, so we have to rescale here
- (left, right) -> m_robotDrive.tankDrive(left / 12., right / 12.),
+ // RamseteCommand passes volts to the callback
+ m_robotDrive::tankDriveVolts,
m_robotDrive
);
// Run path following command, then stop at the end.
- return ramseteCommand.andThen(() -> m_robotDrive.tankDrive(0, 0));
+ return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
index 053fad9..ecb640c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
@@ -19,7 +19,6 @@
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kDriveKinematics;
import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kEncoderDistancePerPulse;
import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kGyroReversed;
import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftEncoderPorts;
@@ -57,7 +56,7 @@
private final Gyro m_gyro = new ADXRS450_Gyro();
// Odometry class for tracking robot pose
- DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(kDriveKinematics);
+ private final DifferentialDriveOdometry m_odometry;
/**
* Creates a new DriveSubsystem.
@@ -66,16 +65,16 @@
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+
+ resetEncoders();
+ m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));
}
@Override
public void periodic() {
// Update the odometry in the periodic block
- m_odometry.update(new Rotation2d(getHeading()),
- new DifferentialDriveWheelSpeeds(
- m_leftEncoder.getRate(),
- m_rightEncoder.getRate()
- ));
+ m_odometry.update(Rotation2d.fromDegrees(getHeading()), m_leftEncoder.getDistance(),
+ m_rightEncoder.getDistance());
}
/**
@@ -88,12 +87,22 @@
}
/**
+ * Returns the current wheel speeds of the robot.
+ *
+ * @return The current wheel speeds.
+ */
+ public DifferentialDriveWheelSpeeds getWheelSpeeds() {
+ return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
+ }
+
+ /**
* Resets the odometry to the specified pose.
*
* @param pose The pose to which to set the odometry.
*/
public void resetOdometry(Pose2d pose) {
- m_odometry.resetPosition(pose);
+ resetEncoders();
+ m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
}
/**
@@ -107,14 +116,14 @@
}
/**
- * Drives the robot using tank controls. Does not square inputs to enable composition with
- * external controllers.
+ * Controls the left and right sides of the drive directly with voltages.
*
- * @param left the commanded left output
- * @param right the commanded right output
+ * @param leftVolts the commanded left output
+ * @param rightVolts the commanded right output
*/
- public void tankDrive(double left, double right) {
- m_drive.tankDrive(left, right, false);
+ public void tankDriveVolts(double leftVolts, double rightVolts) {
+ m_leftMotors.setVoltage(leftVolts);
+ m_rightMotors.setVoltage(-rightVolts);
}
/**
@@ -131,7 +140,7 @@
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
- return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
@@ -174,7 +183,7 @@
* @return the robot's heading in degrees, from 180 to 180
*/
public double getHeading() {
- return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1. : 1.);
+ return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
}
/**
@@ -183,6 +192,6 @@
* @return The turn rate of the robot, in degrees per second
*/
public double getTurnRate() {
- return m_gyro.getRate() * (kGyroReversed ? -1. : 1.);
+ return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0);
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
index 0ec09f0..828a94b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
@@ -37,7 +37,7 @@
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
);
- private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics);
+ private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics, getAngle());
public Drivetrain() {
m_gyro.reset();
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
index 58a0c8d..2969fd4 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
@@ -8,7 +8,8 @@
package edu.wpi.first.wpilibj.examples.swervebot;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -23,11 +24,11 @@
private static final double kModuleMaxAngularAcceleration
= 2 * Math.PI; // radians per second squared
- private final Spark m_driveMotor;
- private final Spark m_turningMotor;
+ private final SpeedController m_driveMotor;
+ private final SpeedController m_turningMotor;
private final Encoder m_driveEncoder = new Encoder(0, 1);
- private final Encoder m_turningEncoder = new Encoder(0, 1);
+ private final Encoder m_turningEncoder = new Encoder(2, 3);
private final PIDController m_drivePIDController = new PIDController(1, 0, 0);
@@ -42,8 +43,8 @@
* @param turningMotorChannel ID for the turning motor.
*/
public SwerveModule(int driveMotorChannel, int turningMotorChannel) {
- m_driveMotor = new Spark(driveMotorChannel);
- m_turningMotor = new Spark(turningMotorChannel);
+ m_driveMotor = new PWMVictorSPX(driveMotorChannel);
+ m_turningMotor = new PWMVictorSPX(turningMotorChannel);
// Set the distance per pulse for the drive encoder. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java
new file mode 100644
index 0000000..19fc601
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.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.examples.swervecontrollercommand;
+
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kFrontLeftDriveMotorPort = 0;
+ public static final int kRearLeftDriveMotorPort = 2;
+ public static final int kFrontRightDriveMotorPort = 4;
+ public static final int kRearRightDriveMotorPort = 6;
+
+ public static final int kFrontLeftTurningMotorPort = 1;
+ public static final int kRearLeftTurningMotorPort = 3;
+ public static final int kFrontRightTurningMotorPort = 5;
+ public static final int kRearRightTurningMotorPort = 7;
+
+ public static final int[] kFrontLeftTurningEncoderPorts = new int[]{0, 1};
+ public static final int[] kRearLeftTurningEncoderPorts = new int[]{2, 3};
+ public static final int[] kFrontRightTurningEncoderPorts = new int[]{4, 5};
+ public static final int[] kRearRightTurningEncoderPorts = new int[]{5, 6};
+
+ public static final boolean kFrontLeftTurningEncoderReversed = false;
+ public static final boolean kRearLeftTurningEncoderReversed = true;
+ public static final boolean kFrontRightTurningEncoderReversed = false;
+ public static final boolean kRearRightTurningEncoderReversed = true;
+
+ public static final int[] kFrontLeftDriveEncoderPorts = new int[]{7, 8};
+ public static final int[] kRearLeftDriveEncoderPorts = new int[]{9, 10};
+ public static final int[] kFrontRightDriveEncoderPorts = new int[]{11, 12};
+ public static final int[] kRearRightDriveEncoderPorts = new int[]{13, 14};
+
+ public static final boolean kFrontLeftDriveEncoderReversed = false;
+ public static final boolean kRearLeftDriveEncoderReversed = true;
+ public static final boolean kFrontRightDriveEncoderReversed = false;
+ public static final boolean kRearRightDriveEncoderReversed = true;
+
+
+ public static final double kTrackWidth = 0.5;
+ //Distance between centers of right and left wheels on robot
+ public static final double kWheelBase = 0.7;
+ //Distance between front and back wheels on robot
+ public static final SwerveDriveKinematics kDriveKinematics =
+ new SwerveDriveKinematics(
+ new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+
+ public static final boolean kGyroReversed = false;
+
+ // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+ // These characterization values MUST be determined either experimentally or theoretically
+ // for *your* robot's drive.
+ // The RobotPy Characterization Toolsuite provides a convenient tool for obtaining these
+ // values for your robot.
+ public static final double ksVolts = 1;
+ public static final double kvVoltSecondsPerMeter = 0.8;
+ public static final double kaVoltSecondsSquaredPerMeter = 0.15;
+
+ }
+
+ public static final class ModuleConstants {
+ public static final double kMaxModuleAngularSpeedRadiansPerSecond = 2 * Math.PI;
+ public static final double kMaxModuleAngularAccelerationRadiansPerSecondSquared = 2 * Math.PI;
+
+ public static final int kEncoderCPR = 1024;
+ public static final double kWheelDiameterMeters = 0.15;
+ public static final double kDriveEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
+
+ public static final double kTurningEncoderDistancePerPulse =
+ // Assumes the encoders are on a 1:1 reduction with the module shaft.
+ (2 * Math.PI) / (double) kEncoderCPR;
+
+ public static final double kPModuleTurningController = 1;
+
+ public static final double kPModuleDriveController = 1;
+
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 1;
+
+ }
+
+ public static final class AutoConstants {
+ public static final double kMaxSpeedMetersPerSecond = 3;
+ public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+ public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
+ public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
+
+ public static final double kPXController = 1;
+ public static final double kPYController = 1;
+ public static final double kPThetaController = 1;
+
+ //Constraint for the motion profilied robot angle controller
+ public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
+ new TrapezoidProfile.Constraints(kMaxAngularSpeedRadiansPerSecond,
+ kMaxAngularSpeedRadiansPerSecondSquared);
+
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java
similarity index 75%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java
index d7f6e26..5c8a1da 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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.templates.iterative;
+package edu.wpi.first.wpilibj.examples.swervecontrollercommand;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java
new file mode 100644
index 0000000..3d41ba4
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.swervecontrollercommand;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
new file mode 100644
index 0000000..d0fcc71
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
@@ -0,0 +1,125 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.swervecontrollercommand;
+
+import java.util.List;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+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.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kPThetaController;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kPXController;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kPYController;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kThetaControllerConstraints;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kDriveKinematics;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.OIConstants.kDriverControllerPort;
+
+/*
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems
+ private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+
+ // The driver's controller
+ XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+
+ // Configure default commands
+ // Set the default drive command to split-stick arcade drive
+ m_robotDrive.setDefaultCommand(
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ new RunCommand(() -> m_robotDrive.drive(
+ m_driverController.getY(GenericHID.Hand.kLeft),
+ m_driverController.getX(GenericHID.Hand.kRight),
+ m_driverController.getX(GenericHID.Hand.kLeft), false)));
+
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
+ * {@link JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ }
+
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ // Create config for trajectory
+ TrajectoryConfig config =
+ new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared)
+ // Add kinematics to ensure max speed is actually obeyed
+ .setKinematics(kDriveKinematics);
+
+ // An example trajectory to follow. All units in meters.
+ Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
+ // Start at the origin facing the +X direction
+ new Pose2d(0, 0, new Rotation2d(0)),
+ // Pass through these two interior waypoints, making an 's' curve path
+ List.of(
+ new Translation2d(1, 1),
+ new Translation2d(2, - 1)
+ ),
+ // End 3 meters straight ahead of where we started, facing forward
+ new Pose2d(3, 0, new Rotation2d(0)),
+ config
+ );
+
+ SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
+ exampleTrajectory,
+ m_robotDrive::getPose, //Functional interface to feed supplier
+ kDriveKinematics,
+
+ //Position controllers
+ new PIDController(kPXController, 0, 0),
+ new PIDController(kPYController, 0, 0),
+ new ProfiledPIDController(kPThetaController, 0, 0, kThetaControllerConstraints),
+
+ m_robotDrive::setModuleStates,
+
+ m_robotDrive
+
+ );
+
+ // Run path following command, then stop at the end.
+ return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000..ba80a15
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.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.examples.swervecontrollercommand.subsystems;
+
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kDriveKinematics;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftDriveEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftDriveEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftDriveMotorPort;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftTurningEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftTurningEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftTurningMotorPort;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightDriveEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightDriveEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightDriveMotorPort;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightTurningEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightTurningEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightTurningMotorPort;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kGyroReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftDriveEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftDriveEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftDriveMotorPort;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftTurningEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftTurningEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftTurningMotorPort;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightDriveEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightDriveEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightDriveMotorPort;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightTurningEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightTurningEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightTurningMotorPort;
+
+@SuppressWarnings("PMD.ExcessiveImports")
+public class DriveSubsystem extends SubsystemBase {
+ //Robot swerve modules
+ private final SwerveModule m_frontLeft = new SwerveModule(kFrontLeftDriveMotorPort,
+ kFrontLeftTurningMotorPort,
+ kFrontLeftDriveEncoderPorts,
+ kFrontLeftTurningEncoderPorts,
+ kFrontLeftDriveEncoderReversed,
+ kFrontLeftTurningEncoderReversed);
+
+ private final SwerveModule m_rearLeft = new SwerveModule(kRearLeftDriveMotorPort,
+ kRearLeftTurningMotorPort,
+ kRearLeftDriveEncoderPorts,
+ kRearLeftTurningEncoderPorts,
+ kRearLeftDriveEncoderReversed,
+ kRearLeftTurningEncoderReversed);
+
+
+ private final SwerveModule m_frontRight = new SwerveModule(kFrontRightDriveMotorPort,
+ kFrontRightTurningMotorPort,
+ kFrontRightDriveEncoderPorts,
+ kFrontRightTurningEncoderPorts,
+ kFrontRightDriveEncoderReversed,
+ kFrontRightTurningEncoderReversed);
+
+ private final SwerveModule m_rearRight = new SwerveModule(kRearRightDriveMotorPort,
+ kRearRightTurningMotorPort,
+ kRearRightDriveEncoderPorts,
+ kRearRightTurningEncoderPorts,
+ kRearRightDriveEncoderReversed,
+ kRearRightTurningEncoderReversed);
+
+ // The gyro sensor
+ private final Gyro m_gyro = new ADXRS450_Gyro();
+
+ // Odometry class for tracking robot pose
+ SwerveDriveOdometry m_odometry =
+ new SwerveDriveOdometry(kDriveKinematics, getAngle());
+
+ /**
+ * Creates a new DriveSubsystem.
+ */
+ public DriveSubsystem() {}
+
+ /**
+ * Returns the angle of the robot as a Rotation2d.
+ *
+ * @return The angle of the robot.
+ */
+ public Rotation2d getAngle() {
+ // Negating the angle because WPILib gyros are CW positive.
+ return Rotation2d.fromDegrees(m_gyro.getAngle() * (kGyroReversed ? 1.0 : -1.0));
+ }
+
+ @Override
+ public void periodic() {
+ // Update the odometry in the periodic block
+ m_odometry.update(
+ new Rotation2d(getHeading()),
+ m_frontLeft.getState(),
+ m_rearLeft.getState(),
+ m_frontRight.getState(),
+ m_rearRight.getState());
+ }
+
+ /**
+ * Returns the currently-estimated pose of the robot.
+ *
+ * @return The pose.
+ */
+ public Pose2d getPose() {
+ return m_odometry.getPoseMeters();
+ }
+
+ /**
+ * Resets the odometry to the specified pose.
+ *
+ * @param pose The pose to which to set the odometry.
+ */
+ public void resetOdometry(Pose2d pose) {
+ m_odometry.resetPosition(pose, getAngle());
+ }
+
+ /**
+ * Method to drive the robot using joystick info.
+ *
+ * @param xSpeed Speed of the robot in the x direction (forward).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
+ * @param fieldRelative Whether the provided x and y speeds are relative to the field.
+ */
+ @SuppressWarnings("ParameterName")
+ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+ var swerveModuleStates = kDriveKinematics.toSwerveModuleStates(
+ fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
+ xSpeed, ySpeed, rot, getAngle())
+ : new ChassisSpeeds(xSpeed, ySpeed, rot)
+ );
+ SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeedMetersPerSecond);
+ m_frontLeft.setDesiredState(swerveModuleStates[0]);
+ m_frontRight.setDesiredState(swerveModuleStates[1]);
+ m_rearLeft.setDesiredState(swerveModuleStates[2]);
+ m_rearRight.setDesiredState(swerveModuleStates[3]);
+ }
+
+ /**
+ * Sets the swerve ModuleStates.
+ *
+ * @param desiredStates The desired SwerveModule states.
+ */
+ public void setModuleStates(SwerveModuleState[] desiredStates) {
+ SwerveDriveKinematics.normalizeWheelSpeeds(desiredStates, kMaxSpeedMetersPerSecond);
+ m_frontLeft.setDesiredState(desiredStates[0]);
+ m_frontRight.setDesiredState(desiredStates[1]);
+ m_rearLeft.setDesiredState(desiredStates[2]);
+ m_rearRight.setDesiredState(desiredStates[3]);
+ }
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ public void resetEncoders() {
+ m_frontLeft.resetEncoders();
+ m_rearLeft.resetEncoders();
+ m_frontRight.resetEncoders();
+ m_rearRight.resetEncoders();
+ }
+
+ /**
+ * Zeroes the heading of the robot.
+ */
+ public void zeroHeading() {
+ m_gyro.reset();
+ }
+
+ /**
+ * Returns the heading of the robot.
+ *
+ * @return the robot's heading in degrees, from 180 to 180
+ */
+ public double getHeading() {
+ return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
+ }
+
+ /**
+ * Returns the turn rate of the robot.
+ *
+ * @return The turn rate of the robot, in degrees per second
+ */
+ public double getTurnRate() {
+ return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java
new file mode 100644
index 0000000..bf33288
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.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.examples.swervecontrollercommand.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kDriveEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kMaxModuleAngularAccelerationRadiansPerSecondSquared;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kPModuleDriveController;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kPModuleTurningController;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kTurningEncoderDistancePerPulse;
+
+public class SwerveModule {
+ private final Spark m_driveMotor;
+ private final Spark m_turningMotor;
+
+ private final Encoder m_driveEncoder;
+ private final Encoder m_turningEncoder;
+
+ private final PIDController m_drivePIDController =
+ new PIDController(kPModuleDriveController, 0, 0);
+
+ //Using a TrapezoidProfile PIDController to allow for smooth turning
+ private final ProfiledPIDController m_turningPIDController
+ = new ProfiledPIDController(kPModuleTurningController, 0, 0,
+ new TrapezoidProfile.Constraints(kMaxModuleAngularSpeedRadiansPerSecond,
+ kMaxModuleAngularAccelerationRadiansPerSecondSquared));
+
+ /**
+ * Constructs a SwerveModule.
+ *
+ * @param driveMotorChannel ID for the drive motor.
+ * @param turningMotorChannel ID for the turning motor.
+ */
+ public SwerveModule(int driveMotorChannel,
+ int turningMotorChannel,
+ int[] driveEncoderPorts,
+ int[] turningEncoderPorts,
+ boolean driveEncoderReversed,
+ boolean turningEncoderReversed) {
+
+ m_driveMotor = new Spark(driveMotorChannel);
+ m_turningMotor = new Spark(turningMotorChannel);
+
+ this.m_driveEncoder = new Encoder(driveEncoderPorts[0], driveEncoderPorts[1]);
+
+ this.m_turningEncoder = new Encoder(turningEncoderPorts[0], turningEncoderPorts[1]);
+
+ // Set the distance per pulse for the drive encoder. We can simply use the
+ // distance traveled for one rotation of the wheel divided by the encoder
+ // resolution.
+ m_driveEncoder.setDistancePerPulse(kDriveEncoderDistancePerPulse);
+
+ //Set whether drive encoder should be reversed or not
+ m_driveEncoder.setReverseDirection(driveEncoderReversed);
+
+ // Set the distance (in this case, angle) per pulse for the turning encoder.
+ // This is the the angle through an entire rotation (2 * wpi::math::pi)
+ // divided by the encoder resolution.
+ m_turningEncoder.setDistancePerPulse(kTurningEncoderDistancePerPulse);
+
+ //Set whether turning encoder should be reversed or not
+ m_turningEncoder.setReverseDirection(turningEncoderReversed);
+
+ // Limit the PID Controller's input range between -pi and pi and set the input
+ // to be continuous.
+ m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
+ }
+
+ /**
+ * Returns the current state of the module.
+ *
+ * @return The current state of the module.
+ */
+ public SwerveModuleState getState() {
+ return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get()));
+ }
+
+ /**
+ * Sets the desired state for the module.
+ *
+ * @param state Desired state with speed and angle.
+ */
+ public void setDesiredState(SwerveModuleState state) {
+ // Calculate the drive output from the drive PID controller.
+ final var driveOutput = m_drivePIDController.calculate(
+ m_driveEncoder.getRate(), state.speedMetersPerSecond);
+
+ // Calculate the turning motor output from the turning PID controller.
+ final var turnOutput = m_turningPIDController.calculate(
+ m_turningEncoder.get(), state.angle.getRadians()
+ );
+
+ // Calculate the turning motor output from the turning PID controller.
+ m_driveMotor.set(driveOutput);
+ m_turningMotor.set(turnOutput);
+ }
+
+ /**
+ * Zeros all the SwerveModule encoders.
+ */
+
+ public void resetEncoders() {
+ m_driveEncoder.reset();
+ m_turningEncoder.reset();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Main.java
similarity index 88%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Main.java
index d7f6e26..3adcee9 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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.templates.iterative;
+package edu.wpi.first.wpilibj.examples.tankdrivexboxcontroller;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java
new file mode 100644
index 0000000..f0a9e76
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.tankdrivexboxcontroller;
+
+import edu.wpi.first.wpilibj.GenericHID.Hand;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class.
+ * Runs the motors with tank steering and an Xbox controller.
+ */
+public class Robot extends TimedRobot {
+ private final PWMVictorSPX m_leftMotor = new PWMVictorSPX(0);
+ private final PWMVictorSPX m_rightMotor = new PWMVictorSPX(1);
+ private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final XboxController m_driverController = new XboxController(0);
+
+ @Override
+ public void teleopPeriodic() {
+ // Drive with tank drive.
+ // That means that the Y axis of the left stick moves the left side
+ // of the robot forward and backward, and the Y axis of the right stick
+ // moves the right side of the robot forward and backward.
+ m_robotDrive.tankDrive(m_driverController.getY(Hand.kLeft),
+ m_driverController.getY(Hand.kRight));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
index e03c66c..2cae288 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -8,6 +8,7 @@
package edu.wpi.first.wpilibj.examples.ultrasonic;
import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.MedianFilter;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -31,6 +32,9 @@
private static final int kRightMotorPort = 1;
private static final int kUltrasonicPort = 0;
+ // median filter to discard outliers; filters over 10 samples
+ private final MedianFilter m_filter = new MedianFilter(10);
+
private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
private final DifferentialDrive m_robotDrive
= new DifferentialDrive(new PWMVictorSPX(kLeftMotorPort),
@@ -43,7 +47,9 @@
@Override
public void teleopPeriodic() {
// sensor returns a value from 0-4095 that is scaled to inches
- double currentDistance = m_ultrasonic.getValue() * kValueToInches;
+ // returned value is filtered with a rolling median filter, since ultrasonics
+ // tend to be quite noisy and susceptible to sudden outliers
+ double currentDistance = m_filter.calculate(m_ultrasonic.getValue()) * kValueToInches;
// convert distance error to a motor speed
double currentSpeed = (kHoldDistance - currentDistance) * kP;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
index 225f3ad..b023429 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
@@ -8,6 +8,7 @@
package edu.wpi.first.wpilibj.examples.ultrasonicpid;
import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.MedianFilter;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.controller.PIDController;
@@ -37,6 +38,9 @@
private static final int kRightMotorPort = 1;
private static final int kUltrasonicPort = 0;
+ // median filter to discard outliers; filters over 5 samples
+ private final MedianFilter m_filter = new MedianFilter(5);
+
private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
private final DifferentialDrive m_robotDrive
= new DifferentialDrive(new PWMVictorSPX(kLeftMotorPort),
@@ -51,8 +55,10 @@
@Override
public void teleopPeriodic() {
+ // returned value is filtered with a rolling median filter, since ultrasonics
+ // tend to be quite noisy and susceptible to sudden outliers
double pidOutput
- = m_pidController.calculate(m_ultrasonic.getAverageVoltage());
+ = m_pidController.calculate(m_filter.calculate(m_ultrasonic.getVoltage()));
m_robotDrive.arcadeDrive(pidOutput, 0);
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java
deleted file mode 100644
index 630be9a..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java
+++ /dev/null
@@ -1,99 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.templates.iterative;
-
-import edu.wpi.first.wpilibj.IterativeRobot;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-/**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the IterativeRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the build.gradle file in the
- * project.
- */
-@SuppressWarnings("deprecation")
-public class Robot extends IterativeRobot {
- private static final String kDefaultAuto = "Default";
- private static final String kCustomAuto = "My Auto";
- private String m_autoSelected;
- private final SendableChooser<String> m_chooser = new SendableChooser<>();
-
- /**
- * This function is run when the robot is first started up and should be
- * used for any initialization code.
- */
- @Override
- public void robotInit() {
- m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
- m_chooser.addOption("My Auto", kCustomAuto);
- SmartDashboard.putData("Auto choices", m_chooser);
- }
-
- /**
- * This function is called every robot packet, no matter the mode. Use
- * this for items like diagnostics that you want ran during disabled,
- * autonomous, teleoperated and test.
- *
- * <p>This runs after the mode specific periodic functions, but before
- * LiveWindow and SmartDashboard integrated updating.
- */
- @Override
- public void robotPeriodic() {
- }
-
- /**
- * This autonomous (along with the chooser code above) shows how to select
- * between different autonomous modes using the dashboard. The sendable
- * chooser code works with the Java SmartDashboard. If you prefer the
- * LabVIEW Dashboard, remove all of the chooser code and uncomment the
- * getString line to get the auto name from the text box below the Gyro
- *
- * <p>You can add additional auto modes by adding additional comparisons to
- * the switch structure below with additional strings. If using the
- * SendableChooser make sure to add them to the chooser code above as well.
- */
- @Override
- public void autonomousInit() {
- m_autoSelected = m_chooser.getSelected();
- // autoSelected = SmartDashboard.getString("Auto Selector",
- // defaultAuto);
- System.out.println("Auto selected: " + m_autoSelected);
- }
-
- /**
- * This function is called periodically during autonomous.
- */
- @Override
- public void autonomousPeriodic() {
- switch (m_autoSelected) {
- case kCustomAuto:
- // Put custom auto code here
- break;
- case kDefaultAuto:
- default:
- // Put default auto code here
- break;
- }
- }
-
- /**
- * This function is called periodically during operator control.
- */
- @Override
- public void teleopPeriodic() {
- }
-
- /**
- * This function is called periodically during test mode.
- */
- @Override
- public void testPeriodic() {
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Main.java
similarity index 88%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Main.java
index d7f6e26..67d8377 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* 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.templates.iterative;
+package edu.wpi.first.wpilibj.templates.oldcommandbased;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/OI.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/OI.java
new file mode 100644
index 0000000..d4c7f91
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/OI.java
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* 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.templates.oldcommandbased;
+
+/**
+ * This class is the glue that binds the controls on the physical operator
+ * interface to the commands and command groups that allow control of the robot.
+ */
+public class OI {
+ //// CREATING BUTTONS
+ // One type of button is a joystick button which is any button on a
+ //// joystick.
+ // You create one by telling it which joystick it's on and which button
+ // number it is.
+ // Joystick stick = new Joystick(port);
+ // Button button = new JoystickButton(stick, buttonNumber);
+
+ // There are a few additional built in buttons you can use. Additionally,
+ // by subclassing Button you can create custom triggers and bind those to
+ // commands the same as any other Button.
+
+ //// TRIGGERING COMMANDS WITH BUTTONS
+ // Once you have a button, it's trivial to bind it to a button in one of
+ // three ways:
+
+ // Start the command when the button is pressed and let it run the command
+ // until it is finished as determined by it's isFinished method.
+ // button.whenPressed(new ExampleCommand());
+
+ // Run the command while the button is being held down and interrupt it once
+ // the button is released.
+ // button.whileHeld(new ExampleCommand());
+
+ // Start the command when the button is released and let it run the command
+ // until it is finished as determined by it's isFinished method.
+ // button.whenReleased(new ExampleCommand());
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Robot.java
new file mode 100644
index 0000000..8f000ac
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Robot.java
@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* 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.templates.oldcommandbased;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.templates.oldcommandbased.commands.ExampleCommand;
+import edu.wpi.first.wpilibj.templates.oldcommandbased.subsystems.ExampleSubsystem;
+
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the TimedRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ public static ExampleSubsystem m_subsystem = new ExampleSubsystem();
+ public static OI m_oi;
+
+ Command m_autonomousCommand;
+ SendableChooser<Command> m_chooser = new SendableChooser<>();
+
+ /**
+ * This function is run when the robot is first started up and should be
+ * used for any initialization code.
+ */
+ @Override
+ public void robotInit() {
+ m_oi = new OI();
+ m_chooser.setDefaultOption("Default Auto", new ExampleCommand());
+ // chooser.addOption("My Auto", new MyAutoCommand());
+ SmartDashboard.putData("Auto mode", m_chooser);
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want ran during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ * You can use it to reset any subsystem information you want to clear when
+ * the robot is disabled.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ Scheduler.getInstance().run();
+ }
+
+ /**
+ * This autonomous (along with the chooser code above) shows how to select
+ * between different autonomous modes using the dashboard. The sendable
+ * chooser code works with the Java SmartDashboard. If you prefer the
+ * LabVIEW Dashboard, remove all of the chooser code and uncomment the
+ * getString code to get the auto name from the text box below the Gyro
+ *
+ * <p>You can add additional auto modes by adding additional commands to the
+ * chooser code above (like the commented example) or additional comparisons
+ * to the switch structure below with additional strings & commands.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_chooser.getSelected();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.start();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ Scheduler.getInstance().run();
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+ Scheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/RobotMap.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/RobotMap.java
new file mode 100644
index 0000000..9f33b02
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/RobotMap.java
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* 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.templates.oldcommandbased;
+
+/**
+ * The RobotMap is a mapping from the ports sensors and actuators are wired into
+ * to a variable name. This provides flexibility changing wiring, makes checking
+ * the wiring easier and significantly reduces the number of magic numbers
+ * floating around.
+ */
+public class RobotMap {
+ // For example to map the left and right motors, you could define the
+ // following variables to use with your drivetrain subsystem.
+ // public static int leftMotor = 1;
+ // public static int rightMotor = 2;
+
+ // If you are using multiple modules, make sure to define both the port
+ // number and the module. For example you with a rangefinder:
+ // public static int rangefinderPort = 1;
+ // public static int rangefinderModule = 1;
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/commands/ExampleCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/commands/ExampleCommand.java
new file mode 100644
index 0000000..0d84987
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/commands/ExampleCommand.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.templates.oldcommandbased.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.templates.oldcommandbased.Robot;
+
+/**
+ * An example command. You can replace me with your own command.
+ */
+public class ExampleCommand extends Command {
+ public ExampleCommand() {
+ // Use requires() here to declare subsystem dependencies
+ requires(Robot.m_subsystem);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/subsystems/ExampleSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/subsystems/ExampleSubsystem.java
new file mode 100644
index 0000000..f3d1de9
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/subsystems/ExampleSubsystem.java
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* 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.templates.oldcommandbased.subsystems;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+/**
+ * An example subsystem. You can replace with me with your own subsystem.
+ */
+public class ExampleSubsystem extends Subsystem {
+ // Put methods for controlling this subsystem
+ // here. Call these from Commands.
+
+
+ @Override
+ protected void initDefaultCommand() {
+ // Set the default command for a subsystem here.
+ // setDefaultCommand(new MySpecialCommand());
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
index 615af87..783f76b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
@@ -33,6 +33,8 @@
public void test() {
}
+ private volatile boolean m_exit;
+
@SuppressWarnings("PMD.CyclomaticComplexity")
@Override
public void startCompetition() {
@@ -41,7 +43,7 @@
// Tell the DS that the robot is ready to be enabled
HAL.observeUserProgramStarting();
- while (!Thread.currentThread().isInterrupted()) {
+ while (!Thread.currentThread().isInterrupted() && !m_exit) {
if (isDisabled()) {
m_ds.InDisabled(true);
disabled();
@@ -77,4 +79,9 @@
}
}
}
+
+ @Override
+ public void endCompetition() {
+ m_exit = true;
+ }
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json
index e4dbc1d..e0ebce7 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json
@@ -1,15 +1,5 @@
[
{
- "name": "Iterative Robot",
- "description": "Iterative style",
- "tags": [
- "Iterative"
- ],
- "foldername": "iterative",
- "gradlebase": "java",
- "mainclass": "Main"
- },
- {
"name": "Timed Robot",
"description": "Timed style",
"tags": [
@@ -17,7 +7,8 @@
],
"foldername": "timed",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Timed Skeleton (Advanced)",
@@ -27,7 +18,8 @@
],
"foldername": "timedskeleton",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "RobotBase Skeleton (Advanced)",
@@ -37,7 +29,8 @@
],
"foldername": "robotbaseskeleton",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Command Robot",
@@ -47,6 +40,18 @@
],
"foldername": "commandbased",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "Old Command Robot",
+ "description": "Old-command style (deprecated)",
+ "tags": [
+ "Command"
+ ],
+ "foldername": "oldcommandbased",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 1
}
]
diff --git a/wpilibjIntegrationTests/build.gradle b/wpilibjIntegrationTests/build.gradle
index 2fc6793..ec5714e 100644
--- a/wpilibjIntegrationTests/build.gradle
+++ b/wpilibjIntegrationTests/build.gradle
@@ -20,17 +20,17 @@
}
dependencies {
- compile project(':wpilibj')
- compile project(':hal')
- compile project(':wpiutil')
- compile project(':ntcore')
- compile project(':cscore')
- compile project(':cameraserver')
- compile 'junit:junit:4.11'
- testCompile 'org.hamcrest:hamcrest-all:1.3'
- compile 'com.googlecode.junit-toolbox:junit-toolbox:2.0'
- compile 'org.apache.ant:ant:1.9.4'
- compile 'org.apache.ant:ant-junit:1.9.4'
+ implementation project(':wpilibj')
+ implementation project(':hal')
+ implementation project(':wpiutil')
+ implementation project(':ntcore')
+ implementation project(':cscore')
+ implementation project(':cameraserver')
+ implementation 'junit:junit:4.11'
+ testImplementation 'org.hamcrest:hamcrest-all:1.3'
+ implementation 'com.googlecode.junit-toolbox:junit-toolbox:2.0'
+ implementation 'org.apache.ant:ant:1.9.4'
+ implementation 'org.apache.ant:ant-junit:1.9.4'
}
build.dependsOn shadowJar
diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AbstractInterruptTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AbstractInterruptTest.java
index 0c27ee7..de54234 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AbstractInterruptTest.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AbstractInterruptTest.java
@@ -137,7 +137,7 @@
// Delay until the interrupt is complete
while (!function.m_interruptComplete.get()) {
- Timer.delay(.005);
+ Timer.delay(0.005);
}
@@ -176,7 +176,7 @@
setInterruptHigh();
// Wait for the interrupt to complete before moving on
while (!function.m_interruptComplete.getAndSet(false)) {
- Timer.delay(.005);
+ Timer.delay(0.005);
}
}
// Then
@@ -194,7 +194,7 @@
// Given
getInterruptable().requestInterrupts();
- final double synchronousDelay = synchronousTimeout / 2.;
+ final double synchronousDelay = synchronousTimeout / 2.0;
final Runnable runnable = () -> {
Timer.delay(synchronousDelay);
setInterruptLow();
@@ -215,7 +215,7 @@
// The test will not have timed out and:
final double interruptRunTime = (stopTimeStamp - startTimeStamp) * 1e-6;
assertEquals("The interrupt did not run for the expected amount of time (units in seconds)",
- synchronousDelay, interruptRunTime, .1);
+ synchronousDelay, interruptRunTime, 0.1);
}
@Test(timeout = (long) (synchronousTimeout * 1e3))
@@ -236,7 +236,7 @@
// Given
getInterruptable().requestInterrupts();
- final double synchronousDelay = synchronousTimeout / 2.;
+ final double synchronousDelay = synchronousTimeout / 2.0;
final Runnable runnable = () -> {
Timer.delay(synchronousDelay);
setInterruptLow();
@@ -258,7 +258,7 @@
getInterruptable().requestInterrupts();
getInterruptable().setUpSourceEdge(false, true);
- final double synchronousDelay = synchronousTimeout / 2.;
+ final double synchronousDelay = synchronousTimeout / 2.0;
final Runnable runnable = () -> {
Timer.delay(synchronousDelay);
setInterruptHigh();
@@ -290,7 +290,7 @@
setInterruptHigh();
// Wait for the interrupt to complete before moving on
while (!function.m_interruptComplete.getAndSet(false)) {
- Timer.delay(.005);
+ Timer.delay(0.005);
}
}
getInterruptable().disableInterrupts();
@@ -300,7 +300,7 @@
setInterruptLow();
setInterruptHigh();
// Just wait because the interrupt should not fire
- Timer.delay(.005);
+ Timer.delay(0.005);
}
// Then
diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometerTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometerTest.java
index 8ac65f6..29ac6e4 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometerTest.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometerTest.java
@@ -61,7 +61,7 @@
for (double i = 0.0; i < 360.0; i = i + 1.0) {
m_potSource.setAngle(i);
m_potSource.setMaxVoltage(RobotController.getVoltage5V());
- Timer.delay(.02);
+ Timer.delay(0.02);
assertEquals(i, m_pot.get(), DOUBLE_COMPARISON_DELTA);
}
}
diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DIOCrossConnectTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DIOCrossConnectTest.java
index 5f59b5e..14eed0c 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DIOCrossConnectTest.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DIOCrossConnectTest.java
@@ -87,7 +87,7 @@
public void testSetHigh() {
dio.getOutput().set(true);
assertTrue("DIO Not High after no delay", dio.getInput().get());
- Timer.delay(.02);
+ Timer.delay(0.02);
assertTrue("DIO Not High after .05s delay", dio.getInput().get());
}
@@ -98,7 +98,7 @@
public void testSetLow() {
dio.getOutput().set(false);
assertFalse("DIO Not Low after no delay", dio.getInput().get());
- Timer.delay(.02);
+ Timer.delay(0.02);
assertFalse("DIO Not Low after .05s delay", dio.getInput().get());
}
diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/GyroTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/GyroTest.java
index 3a5e1d2..1fc828c 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/GyroTest.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/GyroTest.java
@@ -56,7 +56,7 @@
public void testInitial(AnalogGyro gyro) {
double angle = gyro.getAngle();
- assertEquals(errorMessage(angle, 0), 0, angle, .5);
+ assertEquals(errorMessage(angle, 0), 0, angle, 0.5);
}
/**
@@ -68,7 +68,7 @@
// Set angle
for (int i = 0; i < 5; i++) {
m_tpcam.getPan().set(0);
- Timer.delay(.1);
+ Timer.delay(0.1);
}
Timer.delay(0.5);
@@ -99,7 +99,7 @@
gyro.reset();
Timer.delay(0.25);
double angle = gyro.getAngle();
- assertEquals(errorMessage(angle, 0), 0, angle, .5);
+ assertEquals(errorMessage(angle, 0), 0, angle, 0.5);
Timer.delay(5);
angle = gyro.getAngle();
assertEquals("After 5 seconds " + errorMessage(angle, 0), 0, angle, 2.0);
diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
index de08259..7fcd01f 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
@@ -23,6 +23,7 @@
import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
import edu.wpi.first.wpilibj.test.AbstractComsSetup;
import edu.wpi.first.wpilibj.test.TestBench;
+import edu.wpi.first.wpiutil.math.MathUtil;
import static org.junit.Assert.assertEquals;
import static org.junit.Assert.assertFalse;
@@ -33,7 +34,7 @@
public class MotorEncoderTest extends AbstractComsSetup {
private static final Logger logger = Logger.getLogger(MotorEncoderTest.class.getName());
- private static final double MOTOR_RUNTIME = .25;
+ private static final double MOTOR_RUNTIME = 0.25;
// private static final List<MotorEncoderFixture> pairs = new
// ArrayList<MotorEncoderFixture>();
@@ -108,7 +109,7 @@
public void testIncrement() {
int startValue = me.getEncoder().get();
- me.getMotor().set(.2);
+ me.getMotor().set(0.2);
Timer.delay(MOTOR_RUNTIME);
int currentValue = me.getEncoder().get();
assertTrue(me.getType() + " Encoder not incremented: start: " + startValue + "; current: "
@@ -124,7 +125,7 @@
public void testDecrement() {
int startValue = me.getEncoder().get();
- me.getMotor().set(-.2);
+ me.getMotor().set(-0.2);
Timer.delay(MOTOR_RUNTIME);
int currentValue = me.getEncoder().get();
assertTrue(me.getType() + " Encoder not decremented: start: " + startValue + "; current: "
@@ -139,7 +140,7 @@
final int counter1Start = me.getCounters()[0].get();
final int counter2Start = me.getCounters()[1].get();
- me.getMotor().set(.75);
+ me.getMotor().set(0.75);
Timer.delay(MOTOR_RUNTIME);
int counter1End = me.getCounters()[0].get();
int counter2End = me.getCounters()[1].get();
@@ -181,8 +182,10 @@
pidController.setIntegratorRange(-0.2, 0.2);
pidController.setSetpoint(1000);
- Notifier pidRunner = new Notifier(
- () -> me.getMotor().set(pidController.calculate(me.getEncoder().getDistance())));
+ Notifier pidRunner = new Notifier(() -> {
+ var speed = pidController.calculate(me.getEncoder().getDistance());
+ me.getMotor().set(MathUtil.clamp(speed, -0.2, 0.2));
+ });
pidRunner.startPeriodic(pidController.getPeriod());
Timer.delay(10.0);
@@ -202,8 +205,10 @@
pidController.setTolerance(200);
pidController.setSetpoint(600);
- Notifier pidRunner =
- new Notifier(() -> me.getMotor().set(filter.calculate(me.getEncoder().getRate()) + 8e-5));
+ Notifier pidRunner = new Notifier(() -> {
+ var speed = filter.calculate(me.getEncoder().getRate());
+ me.getMotor().set(MathUtil.clamp(speed, -0.3, 0.3));
+ });
pidRunner.startPeriodic(pidController.getPeriod());
Timer.delay(10.0);
diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java
index bbb034e..b781060 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java
@@ -34,7 +34,7 @@
*/
@RunWith(Parameterized.class)
public class PDPTest extends AbstractComsSetup {
- private static final Logger logger = Logger.getLogger(PCMTest.class.getName());
+ private static final Logger logger = Logger.getLogger(PDPTest.class.getName());
private static PowerDistributionPanel pdp;
private static MotorEncoderFixture<?> me;
diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeCounterSource.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeCounterSource.java
index 5608491..d67fbdb 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeCounterSource.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeCounterSource.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -107,7 +107,7 @@
Thread.currentThread().interrupt();
}
m_task = new EncoderThread(this);
- Timer.delay(.01);
+ Timer.delay(0.01);
}
/**
diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeEncoderSource.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeEncoderSource.java
index bdb6886..efc69a5 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeEncoderSource.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeEncoderSource.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -131,7 +131,7 @@
Thread.currentThread().interrupt();
}
m_task = new QuadEncoderThread(this);
- Timer.delay(.01);
+ Timer.delay(0.01);
}
/**
diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java
index 28f4d2b..7c49eef 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* 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. */
@@ -232,14 +232,14 @@
// reached then continue to run this loop
for (timeoutIndex = 0; timeoutIndex < (timeout * 100) && !correctState.getAsBoolean();
timeoutIndex++) {
- Timer.delay(.01);
+ Timer.delay(0.01);
}
if (correctState.getAsBoolean()) {
- simpleLog(level, message + " took " + (timeoutIndex * .01) + " seconds");
+ simpleLog(level, message + " took " + (timeoutIndex * 0.01) + " seconds");
} else {
- simpleLog(level, message + " timed out after " + (timeoutIndex * .01) + " seconds");
+ simpleLog(level, message + " timed out after " + (timeoutIndex * 0.01) + " seconds");
}
- return timeoutIndex * .01;
+ return timeoutIndex * 0.01;
}
}
diff --git a/wpiutil/CMakeLists.txt b/wpiutil/CMakeLists.txt
index 0ec1724..93e0191 100644
--- a/wpiutil/CMakeLists.txt
+++ b/wpiutil/CMakeLists.txt
@@ -14,9 +14,9 @@
include(UseJava)
set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
- if(NOT EXISTS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml-simple-0.38.jar")
+ if(NOT EXISTS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml/ejml-simple-0.38.jar")
set(BASE_URL "https://search.maven.org/remotecontent?filepath=")
- set(JAR_ROOT "${CMAKE_BINARY_DIR}/wpiutil/thirdparty")
+ set(JAR_ROOT "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml")
message(STATUS "Downloading EJML jarfiles...")
@@ -38,8 +38,27 @@
message(STATUS "All files downloaded.")
endif()
+ if(NOT EXISTS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/jackson-core-2.10.0.jar")
+ set(BASE_URL "https://search.maven.org/remotecontent?filepath=")
+ set(JAR_ROOT "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson")
+
+ message(STATUS "Downloading Jackson jarfiles...")
+
+ file(DOWNLOAD "${BASE_URL}com/fasterxml/jackson/core/jackson-core/2.10.0/jackson-core-2.10.0.jar"
+ "${JAR_ROOT}/jackson-core-2.10.0.jar")
+ file(DOWNLOAD "${BASE_URL}com/fasterxml/jackson/core/jackson-databind/2.10.0/jackson-databind-2.10.0.jar"
+ "${JAR_ROOT}/jackson-databind-2.10.0.jar")
+ file(DOWNLOAD "${BASE_URL}com/fasterxml/jackson/core/jackson-annotations/2.10.0/jackson-annotations-2.10.0.jar"
+ "${JAR_ROOT}/jackson-annotations-2.10.0.jar")
+
+ message(STATUS "All files downloaded.")
+ endif()
+
file(GLOB EJML_JARS
- ${CMAKE_BINARY_DIR}/wpiutil/thirdparty/*.jar)
+ ${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml/*.jar)
+
+ file(GLOB JACKSON_JARS
+ ${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar)
set(CMAKE_JAVA_INCLUDE_PATH wpiutil.jar ${EJML_JARS})
@@ -51,9 +70,9 @@
if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
set(CMAKE_JAVA_COMPILE_FLAGS "-h" "${CMAKE_CURRENT_BINARY_DIR}/jniheaders")
- add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} OUTPUT_NAME wpiutil)
+ add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} ${JACKSON_JARS} OUTPUT_NAME wpiutil)
else()
- add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} OUTPUT_NAME wpiutil GENERATE_NATIVE_HEADERS wpiutil_jni_headers)
+ add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} ${JACKSON_JARS} OUTPUT_NAME wpiutil GENERATE_NATIVE_HEADERS wpiutil_jni_headers)
endif()
get_property(WPIUTIL_JAR_FILE TARGET wpiutil_jar PROPERTY JAR_FILE)
diff --git a/wpiutil/build.gradle b/wpiutil/build.gradle
index 588316e..a79bae4 100644
--- a/wpiutil/build.gradle
+++ b/wpiutil/build.gradle
@@ -242,7 +242,10 @@
}
dependencies {
- compile "org.ejml:ejml-simple:0.38"
+ api "org.ejml:ejml-simple:0.38"
+ api "com.fasterxml.jackson.core:jackson-annotations:2.10.0"
+ api "com.fasterxml.jackson.core:jackson-core:2.10.0"
+ api "com.fasterxml.jackson.core:jackson-databind:2.10.0"
}
def wpilibNumberFileInput = file("src/generate/GenericNumber.java.in")
diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtils.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
similarity index 94%
rename from wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtils.java
rename to wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
index 2f91065..cf57834 100644
--- a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtils.java
+++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
@@ -7,8 +7,8 @@
package edu.wpi.first.wpiutil.math;
-public final class MathUtils {
- private MathUtils() {
+public final class MathUtil {
+ private MathUtil() {
throw new AssertionError("utility class");
}
diff --git a/wpiutil/src/main/native/include/wpi/Algorithm.h b/wpiutil/src/main/native/include/wpi/Algorithm.h
new file mode 100644
index 0000000..ec8dc95
--- /dev/null
+++ b/wpiutil/src/main/native/include/wpi/Algorithm.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <vector>
+
+namespace wpi {
+
+// Binary insortion into vector; std::log(n) efficiency.
+template <typename T>
+typename std::vector<T>::iterator insert_sorted(std::vector<T>& vec,
+ T const& item) {
+ return vec.insert(std::upper_bound(vec.begin(), vec.end(), item), item);
+}
+} // namespace wpi
diff --git a/wpiutil/src/main/native/include/wpi/MathExtras.h b/wpiutil/src/main/native/include/wpi/MathExtras.h
index 4dda434..52a56a1 100644
--- a/wpiutil/src/main/native/include/wpi/MathExtras.h
+++ b/wpiutil/src/main/native/include/wpi/MathExtras.h
@@ -838,6 +838,13 @@
return SaturatingAdd(A, Product, &Overflowed);
}
+// Typesafe implementation of the signum function.
+// Returns -1 if negative, 1 if positive, 0 if 0.
+template <typename T>
+constexpr int sgn(T val) {
+ return (T(0) < val) - (val < T(0));
+}
+
} // namespace wpi
#endif