Merge commit '9f561b6053b71dec64d3cccfb988f22aa4288768' into HEAD
Update WPILib to 2020.1.2 kickoff release and update the ni-libraries to
2020v10.
Change-Id: I6527d026be4390d400078067cd35395805d74cd5
diff --git a/WORKSPACE b/WORKSPACE
index fb736ee..1385ad7 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -176,12 +176,12 @@
# Generated with:
# git fetch https://github.com/wpilibsuite/ni-libraries master
-# git archive --output=allwpilib_ni-libraries_3293db0251.tar.gz --format=tar.gz 3293db0251
+# git archive --output=allwpilib_ni-libraries_c826046.tar.gz --format=tar.gz c826046
http_archive(
name = "allwpilib_ni_libraries",
build_file = "@//debian:ni-libraries.BUILD",
- sha256 = "f9cdc52294a8963cf0b41d51fdbb8b190062c98ccd4e6953f8b3f77a641915fe",
- url = "http://frc971.org/Build-Dependencies/allwpilib_ni-libraries_3293db0251.tar.gz",
+ sha256 = "59e971854d689b8e60d2b7ede3cc1da911dbc70deeccb9b5306bb7c7aa5102d9",
+ url = "http://frc971.org/Build-Dependencies/allwpilib_ni-libraries_c826046.tar.gz",
)
# Downloaded from:
diff --git a/third_party/allwpilib_2019/.wpilib/wpilib_preferences.json b/third_party/allwpilib_2019/.wpilib/wpilib_preferences.json
index 984952e..39a096c 100644
--- a/third_party/allwpilib_2019/.wpilib/wpilib_preferences.json
+++ b/third_party/allwpilib_2019/.wpilib/wpilib_preferences.json
@@ -1,6 +1,6 @@
{
"enableCppIntellisense": true,
"currentLanguage": "cpp",
- "projectYear": "Beta2020-2",
+ "projectYear": "2020",
"teamNumber": 0
}
diff --git a/third_party/allwpilib_2019/CONTRIBUTING.md b/third_party/allwpilib_2019/CONTRIBUTING.md
index 30988a4..509f2e2 100644
--- a/third_party/allwpilib_2019/CONTRIBUTING.md
+++ b/third_party/allwpilib_2019/CONTRIBUTING.md
@@ -37,7 +37,7 @@
## Coding Guidelines
-WPILib uses modified Google style guides for both C++ and Java, which can be found in the [styleguide repository](https://github.com/wpilibsuite/styleguide). Autoformatters are available for many popular editors at https://github.com/google/styleguide. Running wpiformat is required for all contributions and is enforced by our continuous integration system. We currently use clang-format 5.0 with wpiformat.
+WPILib uses modified Google style guides for both C++ and Java, which can be found in the [styleguide repository](https://github.com/wpilibsuite/styleguide). Autoformatters are available for many popular editors at https://github.com/google/styleguide. Running wpiformat is required for all contributions and is enforced by our continuous integration system. We currently use clang-format 6.0 with wpiformat.
While the library should be fully formatted according to the styles, additional elements of the style guide were not followed when the library was initially created. All new code should follow the guidelines. If you are looking for some easy ramp-up tasks, finding areas that don't follow the style guide and fixing them is very welcome.
diff --git a/third_party/allwpilib_2019/FasterBuilds.md b/third_party/allwpilib_2019/FasterBuilds.md
new file mode 100644
index 0000000..8963442
--- /dev/null
+++ b/third_party/allwpilib_2019/FasterBuilds.md
@@ -0,0 +1,15 @@
+# Faster Builds for Developers
+
+When you run `./gradlew build`, it builds EVERYTHING. This means debug and release builds for desktop and all installed cross compilers. For many developers, this is way too much, and causes much developer pain.
+
+To help with some of these things, common tasks have shortcuts to only build necessary things for common development and testing tasks.
+
+## Development (Desktop)
+
+For projects `wpiutil`, `ntcore`, `cscore`, `hal` `wpilibOldCommands`, `wpilibNewCommands` and `cameraserver`, a `testDesktopJava` and a `testDesktopCpp` task exists. These can be ran with `./gradlew :projectName:task`, and will only build the minimum things required to run those tests.
+
+For `wpilibc`, a `testDesktopCpp` task exists. For `wpilibj`, a `testDesktopJava` task exists.
+
+For `wpilibcExamples`, a `buildDesktopCpp` task exists (These can't be ran, but they can compile).
+
+For `wpilibjExamples`, a `buildDesktopJava` task exists.
diff --git a/third_party/allwpilib_2019/cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp b/third_party/allwpilib_2019/cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp
index 2302b1a..f7647de 100644
--- a/third_party/allwpilib_2019/cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp
+++ b/third_party/allwpilib_2019/cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp
@@ -13,6 +13,7 @@
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableInstance.h>
#include <wpi/DenseMap.h>
+#include <wpi/ManagedStatic.h>
#include <wpi/SmallString.h>
#include <wpi/StringMap.h>
#include <wpi/mutex.h>
@@ -47,8 +48,14 @@
};
CameraServer* CameraServer::GetInstance() {
- static CameraServer instance;
- return &instance;
+ struct Creator {
+ static void* call() { return new CameraServer{}; }
+ };
+ struct Deleter {
+ static void call(void* ptr) { delete static_cast<CameraServer*>(ptr); }
+ };
+ static wpi::ManagedStatic<CameraServer, Creator, Deleter> instance;
+ return &(*instance);
}
static wpi::StringRef MakeSourceValue(CS_Source source,
diff --git a/third_party/allwpilib_2019/cscore/src/main/native/cpp/HttpCameraImpl.cpp b/third_party/allwpilib_2019/cscore/src/main/native/cpp/HttpCameraImpl.cpp
index 5bf7a8a..57c86a8 100644
--- a/third_party/allwpilib_2019/cscore/src/main/native/cpp/HttpCameraImpl.cpp
+++ b/third_party/allwpilib_2019/cscore/src/main/native/cpp/HttpCameraImpl.cpp
@@ -412,7 +412,11 @@
}
bool HttpCameraImpl::CacheProperties(CS_Status* status) const {
+#ifdef _MSC_VER // work around VS2019 16.4.0 bug
+ std::scoped_lock<wpi::mutex> lock(m_mutex);
+#else
std::scoped_lock lock(m_mutex);
+#endif
// Pretty typical set of video modes
m_videoModes.clear();
diff --git a/third_party/allwpilib_2019/cscore/src/main/native/cpp/UnlimitedHandleResource.h b/third_party/allwpilib_2019/cscore/src/main/native/cpp/UnlimitedHandleResource.h
index 6c9a538..200572c 100644
--- a/third_party/allwpilib_2019/cscore/src/main/native/cpp/UnlimitedHandleResource.h
+++ b/third_party/allwpilib_2019/cscore/src/main/native/cpp/UnlimitedHandleResource.h
@@ -78,7 +78,11 @@
template <typename... Args>
THandle UnlimitedHandleResource<THandle, TStruct, typeValue, TMutex>::Allocate(
Args&&... args) {
+#ifdef _MSC_VER // work around VS2019 16.4.0 bug
+ std::scoped_lock<TMutex> lock(m_handleMutex);
+#else
std::scoped_lock sync(m_handleMutex);
+#endif
size_t i;
for (i = 0; i < m_structures.size(); i++) {
if (m_structures[i] == nullptr) {
diff --git a/third_party/allwpilib_2019/docs/build.gradle b/third_party/allwpilib_2019/docs/build.gradle
index 0505307..10ba02b 100644
--- a/third_party/allwpilib_2019/docs/build.gradle
+++ b/third_party/allwpilib_2019/docs/build.gradle
@@ -125,7 +125,6 @@
ext.entryPoint = "$destinationDir/index.html"
if (JavaVersion.current().isJava11Compatible()) {
- options.addBooleanOption('-no-module-directories', true)
doLast {
// This is a work-around for https://bugs.openjdk.java.net/browse/JDK-8211194. Can be removed once that issue is fixed on JDK's side
// Since JDK 11, package-list is missing from javadoc output files and superseded by element-list file, but a lot of external tools still need it
diff --git a/third_party/allwpilib_2019/hal/src/generate/Instances.txt b/third_party/allwpilib_2019/hal/src/generate/Instances.txt
index e7e9fea..bc30057 100644
--- a/third_party/allwpilib_2019/hal/src/generate/Instances.txt
+++ b/third_party/allwpilib_2019/hal/src/generate/Instances.txt
@@ -3,6 +3,7 @@
kLanguage_Java = 3
kLanguage_Python = 4
kLanguage_DotNet = 5
+kLanguage_Kotlin = 6
kCANPlugin_BlackJagBridge = 1
kCANPlugin_2CAN = 2
kFramework_Iterative = 1
@@ -41,4 +42,11 @@
kADXL345_SPI = 1
kADXL345_I2C = 2
kCommand_Scheduler = 1
+kCommand2_Scheduler = 2
kSmartDashboard_Instance = 1
+kKinematics_DifferentialDrive = 1
+kKinematics_MecanumDrive = 2
+kKinematics_SwerveDrive = 3
+kOdometry_DifferentialDrive = 1
+kOdometry_MecanumDrive = 2
+kOdometry_SwerveDrive = 3
diff --git a/third_party/allwpilib_2019/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java b/third_party/allwpilib_2019/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java
index c473181..c203213 100644
--- a/third_party/allwpilib_2019/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java
+++ b/third_party/allwpilib_2019/hal/src/main/java/edu/wpi/first/hal/SPIJNI.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. */
@@ -61,4 +61,6 @@
double timeout);
public static native int spiGetAutoDroppedCount(int port);
+
+ public static native void spiConfigureAutoStall(int port, int csToSclkTicks, int stallTicks, int pow2BytesPerRead);
}
diff --git a/third_party/allwpilib_2019/hal/src/main/java/edu/wpi/first/hal/sim/DriverStationSim.java b/third_party/allwpilib_2019/hal/src/main/java/edu/wpi/first/hal/sim/DriverStationSim.java
index f87051f..d14af9f 100644
--- a/third_party/allwpilib_2019/hal/src/main/java/edu/wpi/first/hal/sim/DriverStationSim.java
+++ b/third_party/allwpilib_2019/hal/src/main/java/edu/wpi/first/hal/sim/DriverStationSim.java
@@ -79,6 +79,15 @@
DriverStationDataJNI.notifyNewData();
}
+ /**
+ * Toggles suppression of DriverStation.reportError and reportWarning messages.
+ *
+ * @param shouldSend If false then messages will will be suppressed.
+ */
+ public void setSendError(boolean shouldSend) {
+ DriverStationDataJNI.setSendError(shouldSend);
+ }
+
public void resetData() {
DriverStationDataJNI.resetData();
}
diff --git a/third_party/allwpilib_2019/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DriverStationDataJNI.java b/third_party/allwpilib_2019/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DriverStationDataJNI.java
index 2d41ca7..ec19735 100644
--- a/third_party/allwpilib_2019/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DriverStationDataJNI.java
+++ b/third_party/allwpilib_2019/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DriverStationDataJNI.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. */
@@ -49,5 +49,7 @@
public static native void registerAllCallbacks(NotifyCallback callback, boolean initialNotify);
public static native void notifyNewData();
+ public static native void setSendError(boolean shouldSend);
+
public static native void resetData();
}
diff --git a/third_party/allwpilib_2019/hal/src/main/native/athena/AddressableLED.cpp b/third_party/allwpilib_2019/hal/src/main/native/athena/AddressableLED.cpp
index 9ecc4c4..64b6457 100644
--- a/third_party/allwpilib_2019/hal/src/main/native/athena/AddressableLED.cpp
+++ b/third_party/allwpilib_2019/hal/src/main/native/athena/AddressableLED.cpp
@@ -9,6 +9,8 @@
#include <cstring>
+#include <FRC_FPGA_ChipObject/fpgainterfacecapi/NiFpga_HMB.h>
+
#include "ConstantsInternal.h"
#include "DigitalInternal.h"
#include "HALInitializer.h"
@@ -19,14 +21,6 @@
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;
@@ -52,43 +46,6 @@
} // 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(
@@ -146,8 +103,8 @@
uint32_t session = led->led->getSystemInterface()->getHandle();
- *status = OpenHostMemoryBuffer(session, "HMB_0_LED", &led->ledBuffer,
- &led->ledBufferSize);
+ *status = NiFpga_OpenHostMemoryBuffer(session, "HMB_0_LED", &led->ledBuffer,
+ &led->ledBufferSize);
if (*status != 0) {
addressableLEDHandles->Free(handle);
diff --git a/third_party/allwpilib_2019/hal/src/main/native/athena/SPI.cpp b/third_party/allwpilib_2019/hal/src/main/native/athena/SPI.cpp
index 80cbf09..37c5f0e 100644
--- a/third_party/allwpilib_2019/hal/src/main/native/athena/SPI.cpp
+++ b/third_party/allwpilib_2019/hal/src/main/native/athena/SPI.cpp
@@ -631,12 +631,21 @@
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_ConfigureSPIAutoStall(HAL_SPIPort port, int32_t csToSclkTicks,
+ int32_t stallTicks, int32_t pow2BytesPerRead,
+ int32_t* status) {
+ std::scoped_lock lock(spiAutoMutex);
+ // FPGA only has one auto SPI engine
+ if (port != spiAutoPort) {
+ *status = INCOMPATIBLE_STATE;
+ return;
+ }
-void* HAL_GetSPISystem() { return spiSystem.get(); }
+ tSPI::tStallConfig stallConfig;
+ stallConfig.CsToSclkTicks = static_cast<uint8_t>(csToSclkTicks);
+ stallConfig.StallTicks = static_cast<uint16_t>(stallTicks);
+ stallConfig.Pow2BytesPerRead = static_cast<uint8_t>(pow2BytesPerRead);
+ spiSystem->writeStallConfig(stallConfig, status);
+}
} // extern "C"
diff --git a/third_party/allwpilib_2019/hal/src/main/native/cpp/jni/SPIJNI.cpp b/third_party/allwpilib_2019/hal/src/main/native/cpp/jni/SPIJNI.cpp
index 27078fd..7962e21 100644
--- a/third_party/allwpilib_2019/hal/src/main/native/cpp/jni/SPIJNI.cpp
+++ b/third_party/allwpilib_2019/hal/src/main/native/cpp/jni/SPIJNI.cpp
@@ -394,4 +394,20 @@
return retval;
}
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiConfigureAutoStall
+ * Signature: (IIII)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiConfigureAutoStall
+ (JNIEnv* env, jclass, jint port, jint csToSclkTicks, jint stallTicks,
+ jint pow2BytesPerRead)
+{
+ int32_t status = 0;
+ HAL_ConfigureSPIAutoStall(static_cast<HAL_SPIPort>(port), csToSclkTicks,
+ stallTicks, pow2BytesPerRead, &status);
+ CheckStatus(env, status);
+}
+
} // extern "C"
diff --git a/third_party/allwpilib_2019/hal/src/main/native/include/hal/ChipObject.h b/third_party/allwpilib_2019/hal/src/main/native/include/hal/ChipObject.h
index 9b321c2..d891ced 100644
--- a/third_party/allwpilib_2019/hal/src/main/native/include/hal/ChipObject.h
+++ b/third_party/allwpilib_2019/hal/src/main/native/include/hal/ChipObject.h
@@ -27,6 +27,7 @@
#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/tHMB.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>
diff --git a/third_party/allwpilib_2019/hal/src/main/native/include/hal/SPI.h b/third_party/allwpilib_2019/hal/src/main/native/include/hal/SPI.h
index 4f1815f..abee379 100644
--- a/third_party/allwpilib_2019/hal/src/main/native/include/hal/SPI.h
+++ b/third_party/allwpilib_2019/hal/src/main/native/include/hal/SPI.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. */
@@ -244,6 +244,19 @@
*/
int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status);
+/**
+ * Configure the Auto SPI Stall time between reads.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
+ * MXP.
+ * @param csToSclkTicks the number of ticks to wait before asserting the cs pin
+ * @param stallTicks the number of ticks to stall for
+ * @param pow2BytesPerRead the number of bytes to read before stalling
+ */
+void HAL_ConfigureSPIAutoStall(HAL_SPIPort port, int32_t csToSclkTicks,
+ int32_t stallTicks, int32_t pow2BytesPerRead,
+ int32_t* status);
+
#ifdef __cplusplus
} // extern "C"
#endif
diff --git a/third_party/allwpilib_2019/hal/src/main/native/include/mockdata/SimCallbackRegistry.h b/third_party/allwpilib_2019/hal/src/main/native/include/mockdata/SimCallbackRegistry.h
index 7190dcf..3e1aeb0 100644
--- a/third_party/allwpilib_2019/hal/src/main/native/include/mockdata/SimCallbackRegistry.h
+++ b/third_party/allwpilib_2019/hal/src/main/native/include/mockdata/SimCallbackRegistry.h
@@ -74,7 +74,11 @@
template <typename... U>
void Invoke(U&&... u) const {
+#ifdef _MSC_VER // work around VS2019 16.4.0 bug
+ std::scoped_lock<wpi::recursive_spinlock> lock(m_mutex);
+#else
std::scoped_lock lock(m_mutex);
+#endif
if (m_callbacks) {
const char* name = GetName();
for (auto&& cb : *m_callbacks)
diff --git a/third_party/allwpilib_2019/hal/src/main/native/sim/SPI.cpp b/third_party/allwpilib_2019/hal/src/main/native/sim/SPI.cpp
index 8c539d3..1c90a98 100644
--- a/third_party/allwpilib_2019/hal/src/main/native/sim/SPI.cpp
+++ b/third_party/allwpilib_2019/hal/src/main/native/sim/SPI.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. */
@@ -18,6 +18,8 @@
} // namespace init
} // namespace hal
+extern "C" {
+
void HAL_InitializeSPI(HAL_SPIPort port, int32_t* status) {
hal::init::CheckInit();
SimSPIData[port].initialized = true;
@@ -63,3 +65,9 @@
int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status) {
return 0;
}
+
+void HAL_ConfigureSPIAutoStall(HAL_SPIPort port, int32_t csToSclkTicks,
+ int32_t stallTicks, int32_t pow2BytesPerRead,
+ int32_t* status) {}
+
+} // extern "C"
diff --git a/third_party/allwpilib_2019/hal/src/main/native/sim/jni/DriverStationDataJNI.cpp b/third_party/allwpilib_2019/hal/src/main/native/sim/jni/DriverStationDataJNI.cpp
index 621650a..82433b3 100644
--- a/third_party/allwpilib_2019/hal/src/main/native/sim/jni/DriverStationDataJNI.cpp
+++ b/third_party/allwpilib_2019/hal/src/main/native/sim/jni/DriverStationDataJNI.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. */
@@ -14,6 +14,7 @@
#include "CallbackStore.h"
#include "edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI.h"
#include "mockdata/DriverStationData.h"
+#include "mockdata/MockHooks.h"
using namespace wpi::java;
@@ -448,6 +449,25 @@
/*
* Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: setSendError
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setSendError
+ (JNIEnv*, jclass, jboolean shouldSend)
+{
+ if (shouldSend) {
+ HALSIM_SetSendError(nullptr);
+ } else {
+ HALSIM_SetSendError([](HAL_Bool isError, int32_t errorCode,
+ HAL_Bool isLVCode, const char* details,
+ const char* location, const char* callStack,
+ HAL_Bool printMsg) { return 1; });
+ }
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
* Method: resetData
* Signature: ()V
*/
diff --git a/third_party/allwpilib_2019/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableInstance.java b/third_party/allwpilib_2019/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableInstance.java
index 29813a5..06dad09 100644
--- a/third_party/allwpilib_2019/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableInstance.java
+++ b/third_party/allwpilib_2019/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableInstance.java
@@ -44,6 +44,7 @@
public static final int kNetModeClient = 0x02;
public static final int kNetModeStarting = 0x04;
public static final int kNetModeFailure = 0x08;
+ public static final int kNetModeLocal = 0x10;
/**
* The default port that network tables operates on.
@@ -676,6 +677,23 @@
}
/**
+ * Starts local-only operation. Prevents calls to startServer or startClient
+ * from taking effect. Has no effect if startServer or startClient
+ * has already been called.
+ */
+ public void startLocal() {
+ NetworkTablesJNI.startLocal(m_handle);
+ }
+
+ /**
+ * Stops local-only operation. startServer or startClient can be called after
+ * this call to start a server or client.
+ */
+ public void stopLocal() {
+ NetworkTablesJNI.stopLocal(m_handle);
+ }
+
+ /**
* Starts a server using the networktables.ini as the persistent file,
* using the default listening address and port.
*/
diff --git a/third_party/allwpilib_2019/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTablesJNI.java b/third_party/allwpilib_2019/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTablesJNI.java
index d24f066..742c0ca 100644
--- a/third_party/allwpilib_2019/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTablesJNI.java
+++ b/third_party/allwpilib_2019/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTablesJNI.java
@@ -139,6 +139,8 @@
public static native void setNetworkIdentity(int inst, String name);
public static native int getNetworkMode(int inst);
+ public static native void startLocal(int inst);
+ public static native void stopLocal(int inst);
public static native void startServer(int inst, String persistFilename, String listenAddress, int port);
public static native void stopServer(int inst);
public static native void startClient(int inst);
diff --git a/third_party/allwpilib_2019/ntcore/src/main/native/cpp/Dispatcher.cpp b/third_party/allwpilib_2019/ntcore/src/main/native/cpp/Dispatcher.cpp
index 8164f45..6d1ead0 100644
--- a/third_party/allwpilib_2019/ntcore/src/main/native/cpp/Dispatcher.cpp
+++ b/third_party/allwpilib_2019/ntcore/src/main/native/cpp/Dispatcher.cpp
@@ -115,6 +115,16 @@
unsigned int DispatcherBase::GetNetworkMode() const { return m_networkMode; }
+void DispatcherBase::StartLocal() {
+ {
+ std::scoped_lock lock(m_user_mutex);
+ if (m_active) return;
+ m_active = true;
+ }
+ m_networkMode = NT_NET_MODE_LOCAL;
+ m_storage.SetDispatcher(this, false);
+}
+
void DispatcherBase::StartServer(
const Twine& persist_filename,
std::unique_ptr<wpi::NetworkAcceptor> acceptor) {
diff --git a/third_party/allwpilib_2019/ntcore/src/main/native/cpp/Dispatcher.h b/third_party/allwpilib_2019/ntcore/src/main/native/cpp/Dispatcher.h
index dfaf4d5..bfddae7 100644
--- a/third_party/allwpilib_2019/ntcore/src/main/native/cpp/Dispatcher.h
+++ b/third_party/allwpilib_2019/ntcore/src/main/native/cpp/Dispatcher.h
@@ -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. */
@@ -48,6 +48,7 @@
virtual ~DispatcherBase();
unsigned int GetNetworkMode() const;
+ void StartLocal();
void StartServer(const Twine& persist_filename,
std::unique_ptr<wpi::NetworkAcceptor> acceptor);
void StartClient();
diff --git a/third_party/allwpilib_2019/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp b/third_party/allwpilib_2019/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp
index 49f34f0..ac00a3c 100644
--- a/third_party/allwpilib_2019/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp
+++ b/third_party/allwpilib_2019/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.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. */
@@ -1370,6 +1370,30 @@
/*
* Class: edu_wpi_first_networktables_NetworkTablesJNI
+ * Method: startLocal
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_startLocal
+ (JNIEnv*, jclass, jint inst)
+{
+ nt::StartLocal(inst);
+}
+
+/*
+ * Class: edu_wpi_first_networktables_NetworkTablesJNI
+ * Method: stopLocal
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_stopLocal
+ (JNIEnv*, jclass, jint inst)
+{
+ nt::StopLocal(inst);
+}
+
+/*
+ * Class: edu_wpi_first_networktables_NetworkTablesJNI
* Method: startServer
* Signature: (ILjava/lang/String;Ljava/lang/String;I)V
*/
diff --git a/third_party/allwpilib_2019/ntcore/src/main/native/cpp/ntcore_c.cpp b/third_party/allwpilib_2019/ntcore/src/main/native/cpp/ntcore_c.cpp
index 7ca68f5..60745fe 100644
--- a/third_party/allwpilib_2019/ntcore/src/main/native/cpp/ntcore_c.cpp
+++ b/third_party/allwpilib_2019/ntcore/src/main/native/cpp/ntcore_c.cpp
@@ -539,6 +539,10 @@
return nt::GetNetworkMode(inst);
}
+void NT_StartLocal(NT_Inst inst) { nt::StartLocal(inst); }
+
+void NT_StopLocal(NT_Inst inst) { nt::StopLocal(inst); }
+
void NT_StartServer(NT_Inst inst, const char* persist_filename,
const char* listen_address, unsigned int port) {
nt::StartServer(inst, persist_filename, listen_address, port);
diff --git a/third_party/allwpilib_2019/ntcore/src/main/native/cpp/ntcore_cpp.cpp b/third_party/allwpilib_2019/ntcore/src/main/native/cpp/ntcore_cpp.cpp
index 5a2b8af..ad8f68b 100644
--- a/third_party/allwpilib_2019/ntcore/src/main/native/cpp/ntcore_cpp.cpp
+++ b/third_party/allwpilib_2019/ntcore/src/main/native/cpp/ntcore_cpp.cpp
@@ -742,6 +742,20 @@
return ii->dispatcher.GetNetworkMode();
}
+void StartLocal(NT_Inst inst) {
+ auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
+ if (!ii) return;
+
+ ii->dispatcher.StartLocal();
+}
+
+void StopLocal(NT_Inst inst) {
+ auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
+ if (!ii) return;
+
+ ii->dispatcher.Stop();
+}
+
void StartServer(StringRef persist_filename, const char* listen_address,
unsigned int port) {
auto ii = InstanceImpl::GetDefault();
diff --git a/third_party/allwpilib_2019/ntcore/src/main/native/include/networktables/NetworkTableInstance.h b/third_party/allwpilib_2019/ntcore/src/main/native/include/networktables/NetworkTableInstance.h
index ee08745..16fc5b2 100644
--- a/third_party/allwpilib_2019/ntcore/src/main/native/include/networktables/NetworkTableInstance.h
+++ b/third_party/allwpilib_2019/ntcore/src/main/native/include/networktables/NetworkTableInstance.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. */
@@ -61,7 +61,8 @@
kNetModeServer = NT_NET_MODE_SERVER,
kNetModeClient = NT_NET_MODE_CLIENT,
kNetModeStarting = NT_NET_MODE_STARTING,
- kNetModeFailure = NT_NET_MODE_FAILURE
+ kNetModeFailure = NT_NET_MODE_FAILURE,
+ kNetModeLocal = NT_NET_MODE_LOCAL
};
/**
@@ -299,6 +300,19 @@
unsigned int GetNetworkMode() const;
/**
+ * Starts local-only operation. Prevents calls to StartServer or StartClient
+ * from taking effect. Has no effect if StartServer or StartClient
+ * has already been called.
+ */
+ void StartLocal();
+
+ /**
+ * Stops local-only operation. StartServer or StartClient can be called after
+ * this call to start a server or client.
+ */
+ void StopLocal();
+
+ /**
* Starts a server using the specified filename, listening address, and port.
*
* @param persist_filename the name of the persist file to use (UTF-8 string,
diff --git a/third_party/allwpilib_2019/ntcore/src/main/native/include/networktables/NetworkTableInstance.inl b/third_party/allwpilib_2019/ntcore/src/main/native/include/networktables/NetworkTableInstance.inl
index 83616d9..2b6607f 100644
--- a/third_party/allwpilib_2019/ntcore/src/main/native/include/networktables/NetworkTableInstance.inl
+++ b/third_party/allwpilib_2019/ntcore/src/main/native/include/networktables/NetworkTableInstance.inl
@@ -81,6 +81,10 @@
return ::nt::GetNetworkMode(m_handle);
}
+inline void NetworkTableInstance::StartLocal() { ::nt::StartLocal(m_handle); }
+
+inline void NetworkTableInstance::StopLocal() { ::nt::StopLocal(m_handle); }
+
inline void NetworkTableInstance::StartServer(const Twine& persist_filename,
const char* listen_address,
unsigned int port) {
diff --git a/third_party/allwpilib_2019/ntcore/src/main/native/include/ntcore_c.h b/third_party/allwpilib_2019/ntcore/src/main/native/include/ntcore_c.h
index 60773ea..1ed4025 100644
--- a/third_party/allwpilib_2019/ntcore/src/main/native/include/ntcore_c.h
+++ b/third_party/allwpilib_2019/ntcore/src/main/native/include/ntcore_c.h
@@ -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. */
@@ -95,6 +95,7 @@
NT_NET_MODE_CLIENT = 0x02, /* running in client mode */
NT_NET_MODE_STARTING = 0x04, /* flag for starting (either client or server) */
NT_NET_MODE_FAILURE = 0x08, /* flag for failure (either client or server) */
+ NT_NET_MODE_LOCAL = 0x10, /* running in local-only mode */
};
/*
@@ -1038,6 +1039,19 @@
unsigned int NT_GetNetworkMode(NT_Inst inst);
/**
+ * Starts local-only operation. Prevents calls to NT_StartServer or
+ * NT_StartClient from taking effect. Has no effect if NT_StartServer or
+ * NT_StartClient has already been called.
+ */
+void NT_StartLocal(NT_Inst inst);
+
+/**
+ * Stops local-only operation. NT_StartServer or NT_StartClient can be called
+ * after this call to start a server or client.
+ */
+void NT_StopLocal(NT_Inst inst);
+
+/**
* Starts a server using the specified filename, listening address, and port.
*
* @param inst instance handle
diff --git a/third_party/allwpilib_2019/ntcore/src/main/native/include/ntcore_cpp.h b/third_party/allwpilib_2019/ntcore/src/main/native/include/ntcore_cpp.h
index d7e91ad..af1ea12 100644
--- a/third_party/allwpilib_2019/ntcore/src/main/native/include/ntcore_cpp.h
+++ b/third_party/allwpilib_2019/ntcore/src/main/native/include/ntcore_cpp.h
@@ -1131,6 +1131,19 @@
unsigned int GetNetworkMode(NT_Inst inst);
/**
+ * Starts local-only operation. Prevents calls to StartServer or StartClient
+ * from taking effect. Has no effect if StartServer or StartClient
+ * has already been called.
+ */
+void StartLocal(NT_Inst inst);
+
+/**
+ * Stops local-only operation. StartServer or StartClient can be called after
+ * this call to start a server or client.
+ */
+void StopLocal(NT_Inst inst);
+
+/**
* Starts a server using the specified filename, listening address, and port.
*
* @param persist_filename the name of the persist file to use (UTF-8 string,
diff --git a/third_party/allwpilib_2019/shared/config.gradle b/third_party/allwpilib_2019/shared/config.gradle
index 20ec083..7952018 100644
--- a/third_party/allwpilib_2019/shared/config.gradle
+++ b/third_party/allwpilib_2019/shared/config.gradle
@@ -8,9 +8,9 @@
wpi {
configureDependencies {
wpiVersion = "-1"
- niLibVersion = "2020.9.1"
+ niLibVersion = "2020.10.1"
opencvVersion = "3.4.7-2"
- googleTestVersion = "1.9.0-3-437e100"
+ googleTestVersion = "1.9.0-4-437e100-1"
imguiVersion = "1.72b-2"
}
}
diff --git a/third_party/allwpilib_2019/shared/cppDesktopTestTask.gradle b/third_party/allwpilib_2019/shared/cppDesktopTestTask.gradle
new file mode 100644
index 0000000..04b7224
--- /dev/null
+++ b/third_party/allwpilib_2019/shared/cppDesktopTestTask.gradle
@@ -0,0 +1,21 @@
+model {
+ tasks {
+ def ts = $.testSuites
+ project.tasks.register('testDesktopCpp') { testTask->
+ def systemArch = getCurrentArch()
+ def found = false
+ ts.each {
+ if (it in GoogleTestTestSuiteSpec && it.name == "${nativeName}Test") {
+ it.binaries.each {
+ if (found) return
+ def arch = it.targetPlatform.name
+ if (arch == systemArch && it.buildType.name == 'debug') {
+ testTask.dependsOn it.tasks.run
+ found = true
+ }
+ }
+ }
+ }
+ }
+ }
+}
diff --git a/third_party/allwpilib_2019/shared/examplecheck.gradle b/third_party/allwpilib_2019/shared/examplecheck.gradle
index 4e7bdc2..3d05c67 100644
--- a/third_party/allwpilib_2019/shared/examplecheck.gradle
+++ b/third_party/allwpilib_2019/shared/examplecheck.gradle
@@ -70,6 +70,7 @@
assert it.tags != null
assert it.foldername != null
assert it.replacename != null
+ assert it.commandversion != null
if (project.isCppCommands) {
assert it.headers != null
assert !it.headers.isEmpty()
diff --git a/third_party/allwpilib_2019/shared/javaDesktopTestTask.gradle b/third_party/allwpilib_2019/shared/javaDesktopTestTask.gradle
new file mode 100644
index 0000000..766fa50
--- /dev/null
+++ b/third_party/allwpilib_2019/shared/javaDesktopTestTask.gradle
@@ -0,0 +1,3 @@
+tasks.register('testDesktopJava') {
+ dependsOn test
+}
diff --git a/third_party/allwpilib_2019/shared/javacpp/setupBuild.gradle b/third_party/allwpilib_2019/shared/javacpp/setupBuild.gradle
index 4e6dba7..04086ad 100644
--- a/third_party/allwpilib_2019/shared/javacpp/setupBuild.gradle
+++ b/third_party/allwpilib_2019/shared/javacpp/setupBuild.gradle
@@ -141,6 +141,9 @@
}
}
+apply from: "${rootDir}/shared/cppDesktopTestTask.gradle"
+apply from: "${rootDir}/shared/javaDesktopTestTask.gradle"
+
tasks.withType(RunTestExecutable) {
args "--gtest_output=xml:test_detail.xml"
outputs.dir outputDir
diff --git a/third_party/allwpilib_2019/shared/jni/setupBuild.gradle b/third_party/allwpilib_2019/shared/jni/setupBuild.gradle
index 7622d0e..6c6148c 100644
--- a/third_party/allwpilib_2019/shared/jni/setupBuild.gradle
+++ b/third_party/allwpilib_2019/shared/jni/setupBuild.gradle
@@ -275,6 +275,9 @@
}
}
+apply from: "${rootDir}/shared/cppDesktopTestTask.gradle"
+apply from: "${rootDir}/shared/javaDesktopTestTask.gradle"
+
ext.getJniSpecClass = {
return JniNativeLibrarySpec
}
diff --git a/third_party/allwpilib_2019/simulation/halsim_gui/src/main/native/cpp/AnalogInputGui.cpp b/third_party/allwpilib_2019/simulation/halsim_gui/src/main/native/cpp/AnalogInputGui.cpp
index 8e97bc1..0918a69 100644
--- a/third_party/allwpilib_2019/simulation/halsim_gui/src/main/native/cpp/AnalogInputGui.cpp
+++ b/third_party/allwpilib_2019/simulation/halsim_gui/src/main/native/cpp/AnalogInputGui.cpp
@@ -24,9 +24,18 @@
bool hasInputs = false;
static int numAnalog = HAL_GetNumAnalogInputs();
static int numAccum = HAL_GetNumAccumulators();
+ bool first = true;
for (int i = 0; i < numAnalog; ++i) {
if (HALSIM_GetAnalogInInitialized(i)) {
hasInputs = true;
+
+ if (!first) {
+ ImGui::Spacing();
+ ImGui::Spacing();
+ } else {
+ first = false;
+ }
+
char name[32];
std::snprintf(name, sizeof(name), "In[%d]", i);
if (i < numAccum && HALSIM_GetAnalogGyroInitialized(i)) {
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
index b34f20e..d82e77f 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
@@ -89,7 +89,7 @@
CommandScheduler() {
- HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler);
+ HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand2_Scheduler);
SendableRegistry.addLW(this, "Scheduler");
}
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java b/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
index 575ebb4..f9337c6 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
@@ -25,10 +25,20 @@
* Creates a new PIDSubsystem.
*
* @param controller the PIDController to use
+ * @param initialPosition the initial setpoint of the subsystem
+ */
+ public PIDSubsystem(PIDController controller, double initialPosition) {
+ setSetpoint(initialPosition);
+ m_controller = requireNonNullParam(controller, "controller", "PIDSubsystem");
+ }
+
+ /**
+ * Creates a new PIDSubsystem. Initial setpoint is zero.
+ *
+ * @param controller the PIDController to use
*/
public PIDSubsystem(PIDController controller) {
- requireNonNullParam(controller, "controller", "PIDSubsystem");
- m_controller = controller;
+ this(controller, 0);
}
@Override
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java b/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
index 076b4b8..96e18a0 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -113,7 +113,7 @@
@Override
public void initialize() {
- m_controller.reset();
+ m_controller.reset(m_measurement.getAsDouble());
}
@Override
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java b/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
index 6b3492d..2385ada 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -27,10 +27,21 @@
* Creates a new ProfiledPIDSubsystem.
*
* @param controller the ProfiledPIDController to use
+ * @param initialPosition the initial goal position of the controller
+ */
+ public ProfiledPIDSubsystem(ProfiledPIDController controller,
+ double initialPosition) {
+ m_controller = requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
+ setGoal(initialPosition);
+ }
+
+ /**
+ * Creates a new ProfiledPIDSubsystem. Initial goal position is zero.
+ *
+ * @param controller the ProfiledPIDController to use
*/
public ProfiledPIDSubsystem(ProfiledPIDController controller) {
- requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
- m_controller = controller;
+ this(controller, 0);
}
@Override
@@ -82,7 +93,7 @@
*/
public void enable() {
m_enabled = true;
- m_controller.reset();
+ m_controller.reset(getMeasurement());
}
/**
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java b/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
index 9ae8ae2..e22a5c4 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
@@ -76,7 +76,6 @@
*/
public void addChild(String name, Sendable child) {
SendableRegistry.addLW(child, getSubsystem(), name);
- SendableRegistry.addChild(this, child);
}
@Override
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java b/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
index b7ec8c1..05e6386 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
@@ -9,6 +9,8 @@
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
/**
* 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.
@@ -20,41 +22,53 @@
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;
- }
+ private boolean m_enabled = true;
/**
* 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
+ * @param initialPosition The initial position of the controlled 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_constraints = requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem");
m_state = new TrapezoidProfile.State(initialPosition, 0);
+ setGoal(initialPosition);
m_period = period;
}
+ /**
+ * Creates a new TrapezoidProfileSubsystem.
+ *
+ * @param constraints The constraints (maximum velocity and acceleration) for the profiles.
+ * @param initialPosition The initial position of the controlled mechanism when the subsystem
+ * is constructed.
+ */
+ public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints,
+ double initialPosition) {
+ this(constraints, initialPosition, 0.02);
+ }
+
+ /**
+ * Creates a new TrapezoidProfileSubsystem.
+ *
+ * @param constraints The constraints (maximum velocity and acceleration) for the profiles.
+ */
+ public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints) {
+ this(constraints, 0, 0.02);
+ }
+
@Override
public void periodic() {
var profile = new TrapezoidProfile(m_constraints, m_goal, m_state);
m_state = profile.calculate(m_period);
- useState(m_state);
+ if (m_enabled) {
+ useState(m_state);
+ }
}
/**
@@ -76,6 +90,20 @@
}
/**
+ * Enable the TrapezoidProfileSubsystem's output.
+ */
+ public void enable() {
+ m_enabled = true;
+ }
+
+ /**
+ * Disable the TrapezoidProfileSubsystem's output.
+ */
+ public void disable() {
+ m_enabled = false;
+ }
+
+ /**
* Users should override this to consume the current state of the motion profile.
*
* @param state The current state of the motion profile.
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
index 83c9721..248368d 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -51,6 +51,13 @@
SequentialCommandGroup Command::BeforeStarting(
std::function<void()> toRun,
std::initializer_list<Subsystem*> requirements) && {
+ return std::move(*this).BeforeStarting(
+ std::move(toRun),
+ wpi::makeArrayRef(requirements.begin(), requirements.end()));
+}
+
+SequentialCommandGroup Command::BeforeStarting(
+ std::function<void()> toRun, wpi::ArrayRef<Subsystem*> requirements) && {
std::vector<std::unique_ptr<Command>> temp;
temp.emplace_back(
std::make_unique<InstantCommand>(std::move(toRun), requirements));
@@ -61,6 +68,13 @@
SequentialCommandGroup Command::AndThen(
std::function<void()> toRun,
std::initializer_list<Subsystem*> requirements) && {
+ return std::move(*this).AndThen(
+ std::move(toRun),
+ wpi::makeArrayRef(requirements.begin(), requirements.end()));
+}
+
+SequentialCommandGroup Command::AndThen(
+ std::function<void()> toRun, wpi::ArrayRef<Subsystem*> requirements) && {
std::vector<std::unique_ptr<Command>> temp;
temp.emplace_back(std::move(*this).TransferOwnership());
temp.emplace_back(
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
index 323ed67..f2cfa95 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -21,6 +21,10 @@
m_requirements.insert(requirements.begin(), requirements.end());
}
+void CommandBase::AddRequirements(wpi::ArrayRef<Subsystem*> requirements) {
+ m_requirements.insert(requirements.begin(), requirements.end());
+}
+
void CommandBase::AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements) {
m_requirements.insert(requirements.begin(), requirements.end());
}
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
index 84d0820..842facb 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,6 +11,7 @@
#include <frc/WPIErrors.h>
#include <frc/smartdashboard/SendableBuilder.h>
#include <frc/smartdashboard/SendableRegistry.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/HALBase.h>
#include <networktables/NetworkTableEntry.h>
#include <wpi/DenseMap.h>
@@ -67,6 +68,8 @@
}
CommandScheduler::CommandScheduler() : m_impl(new Impl) {
+ HAL_Report(HALUsageReporting::kResourceType_Command,
+ HALUsageReporting::kCommand2_Scheduler);
frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler");
}
@@ -245,6 +248,12 @@
}
}
+void CommandScheduler::RegisterSubsystem(wpi::ArrayRef<Subsystem*> subsystems) {
+ for (auto* subsystem : subsystems) {
+ RegisterSubsystem(subsystem);
+ }
+}
+
void CommandScheduler::UnregisterSubsystem(
std::initializer_list<Subsystem*> subsystems) {
for (auto* subsystem : subsystems) {
@@ -252,6 +261,13 @@
}
}
+void CommandScheduler::UnregisterSubsystem(
+ wpi::ArrayRef<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()) {
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
index ef7c40a..ee28ba7 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -20,6 +20,18 @@
AddRequirements(requirements);
}
+FunctionalCommand::FunctionalCommand(std::function<void()> onInit,
+ std::function<void()> onExecute,
+ std::function<void(bool)> onEnd,
+ std::function<bool()> isFinished,
+ wpi::ArrayRef<Subsystem*> requirements)
+ : m_onInit{std::move(onInit)},
+ m_onExecute{std::move(onExecute)},
+ m_onEnd{std::move(onEnd)},
+ m_isFinished{std::move(isFinished)} {
+ AddRequirements(requirements);
+}
+
void FunctionalCommand::Initialize() { m_onInit(); }
void FunctionalCommand::Execute() { m_onExecute(); }
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp
index b199074..6f66c5c 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -15,6 +15,12 @@
AddRequirements(requirements);
}
+InstantCommand::InstantCommand(std::function<void()> toRun,
+ wpi::ArrayRef<Subsystem*> requirements)
+ : m_toRun{std::move(toRun)} {
+ AddRequirements(requirements);
+}
+
InstantCommand::InstantCommand() : m_toRun{[] {}} {}
void InstantCommand::Initialize() { m_toRun(); }
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
index 15fb254..8809803 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -52,6 +52,46 @@
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,
+ wpi::ArrayRef<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,
@@ -74,6 +114,30 @@
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,
+ wpi::ArrayRef<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);
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp
index c4eac81..1b13d75 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -16,6 +16,13 @@
AddRequirements(requirements);
}
+NotifierCommand::NotifierCommand(std::function<void()> toRun,
+ units::second_t period,
+ wpi::ArrayRef<Subsystem*> requirements)
+ : m_toRun(toRun), m_notifier{std::move(toRun)}, m_period{period} {
+ AddRequirements(requirements);
+}
+
NotifierCommand::NotifierCommand(NotifierCommand&& other)
: CommandHelper(std::move(other)),
m_toRun(other.m_toRun),
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
index 4a956f6..8ce3c37 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -23,11 +23,30 @@
PIDCommand::PIDCommand(PIDController controller,
std::function<double()> measurementSource,
+ std::function<double()> setpointSource,
+ std::function<void(double)> useOutput,
+ wpi::ArrayRef<Subsystem*> requirements)
+ : m_controller{controller},
+ m_measurement{std::move(measurementSource)},
+ m_setpoint{std::move(setpointSource)},
+ m_useOutput{std::move(useOutput)} {
+ AddRequirements(requirements);
+}
+
+PIDCommand::PIDCommand(PIDController controller,
+ std::function<double()> measurementSource,
double setpoint, std::function<void(double)> useOutput,
std::initializer_list<Subsystem*> requirements)
: PIDCommand(controller, measurementSource, [setpoint] { return setpoint; },
useOutput, requirements) {}
+PIDCommand::PIDCommand(PIDController controller,
+ std::function<double()> measurementSource,
+ double setpoint, std::function<void(double)> useOutput,
+ wpi::ArrayRef<Subsystem*> requirements)
+ : PIDCommand(controller, measurementSource, [setpoint] { return setpoint; },
+ useOutput, requirements) {}
+
void PIDCommand::Initialize() { m_controller.Reset(); }
void PIDCommand::Execute() {
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
index ce8198a..14d965c 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
@@ -9,8 +9,10 @@
using namespace frc2;
-PIDSubsystem::PIDSubsystem(PIDController controller)
- : m_controller{controller} {}
+PIDSubsystem::PIDSubsystem(PIDController controller, double initialPosition)
+ : m_controller{controller} {
+ SetSetpoint(initialPosition);
+}
void PIDSubsystem::Periodic() {
if (m_enabled) {
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
index 261896f..b1608bf 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -35,6 +35,28 @@
RamseteCommand::RamseteCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::RamseteController controller,
+ frc::SimpleMotorFeedforward<units::meters> feedforward,
+ frc::DifferentialDriveKinematics kinematics,
+ std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
+ frc2::PIDController leftController, frc2::PIDController rightController,
+ std::function<void(volt_t, volt_t)> output,
+ wpi::ArrayRef<Subsystem*> requirements)
+ : m_trajectory(trajectory),
+ m_pose(pose),
+ m_controller(controller),
+ m_feedforward(feedforward),
+ m_kinematics(kinematics),
+ m_speeds(wheelSpeeds),
+ m_leftController(std::make_unique<frc2::PIDController>(leftController)),
+ m_rightController(std::make_unique<frc2::PIDController>(rightController)),
+ m_outputVolts(output),
+ m_usePID(true) {
+ AddRequirements(requirements);
+}
+
+RamseteCommand::RamseteCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::RamseteController controller,
frc::DifferentialDriveKinematics kinematics,
std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
output,
@@ -48,6 +70,22 @@
AddRequirements(requirements);
}
+RamseteCommand::RamseteCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::RamseteController controller,
+ frc::DifferentialDriveKinematics kinematics,
+ std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
+ output,
+ wpi::ArrayRef<Subsystem*> requirements)
+ : m_trajectory(trajectory),
+ m_pose(pose),
+ m_controller(controller),
+ m_kinematics(kinematics),
+ m_outputVel(output),
+ m_usePID(false) {
+ AddRequirements(requirements);
+}
+
void RamseteCommand::Initialize() {
m_prevTime = 0_s;
auto initialState = m_trajectory.Sample(0_s);
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp
index 5c2c755..dff0ffe 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -15,4 +15,10 @@
AddRequirements(requirements);
}
+RunCommand::RunCommand(std::function<void()> toRun,
+ wpi::ArrayRef<Subsystem*> requirements)
+ : m_toRun{std::move(toRun)} {
+ AddRequirements(requirements);
+}
+
void RunCommand::Execute() { m_toRun(); }
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp
index 33407bf..ad54ae0 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -16,6 +16,13 @@
AddRequirements(requirements);
}
+StartEndCommand::StartEndCommand(std::function<void()> onInit,
+ std::function<void()> onEnd,
+ wpi::ArrayRef<Subsystem*> requirements)
+ : m_onInit{std::move(onInit)}, m_onEnd{std::move(onEnd)} {
+ AddRequirements(requirements);
+}
+
StartEndCommand::StartEndCommand(const StartEndCommand& other)
: CommandHelper(other) {
m_onInit = other.m_onInit;
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
index 5e5ecbf..9b30fec 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
@@ -63,5 +63,4 @@
void SubsystemBase::AddChild(std::string name, frc::Sendable* child) {
auto& registry = frc::SendableRegistry::GetInstance();
registry.AddLW(child, GetSubsystem(), name);
- registry.AddChild(this, child);
}
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
index 440675f..d8a7372 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -22,6 +22,12 @@
return *this;
}
+Button Button::WhenPressed(std::function<void()> toRun,
+ wpi::ArrayRef<Subsystem*> requirements) {
+ WhenActive(std::move(toRun), requirements);
+ return *this;
+}
+
Button Button::WhileHeld(Command* command, bool interruptible) {
WhileActiveContinous(command, interruptible);
return *this;
@@ -33,6 +39,12 @@
return *this;
}
+Button Button::WhileHeld(std::function<void()> toRun,
+ wpi::ArrayRef<Subsystem*> requirements) {
+ WhileActiveContinous(std::move(toRun), requirements);
+ return *this;
+}
+
Button Button::WhenHeld(Command* command, bool interruptible) {
WhileActiveOnce(command, interruptible);
return *this;
@@ -49,6 +61,12 @@
return *this;
}
+Button Button::WhenReleased(std::function<void()> toRun,
+ wpi::ArrayRef<Subsystem*> requirements) {
+ WhenInactive(std::move(toRun), requirements);
+ return *this;
+}
+
Button Button::ToggleWhenPressed(Command* command, bool interruptible) {
ToggleWhenActive(command, interruptible);
return *this;
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
index caf188e..5c1a8f7 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -30,6 +30,12 @@
Trigger Trigger::WhenActive(std::function<void()> toRun,
std::initializer_list<Subsystem*> requirements) {
+ return WhenActive(std::move(toRun), wpi::makeArrayRef(requirements.begin(),
+ requirements.end()));
+}
+
+Trigger Trigger::WhenActive(std::function<void()> toRun,
+ wpi::ArrayRef<Subsystem*> requirements) {
return WhenActive(InstantCommand(std::move(toRun), requirements));
}
@@ -52,6 +58,13 @@
Trigger Trigger::WhileActiveContinous(
std::function<void()> toRun,
std::initializer_list<Subsystem*> requirements) {
+ return WhileActiveContinous(
+ std::move(toRun),
+ wpi::makeArrayRef(requirements.begin(), requirements.end()));
+}
+
+Trigger Trigger::WhileActiveContinous(std::function<void()> toRun,
+ wpi::ArrayRef<Subsystem*> requirements) {
return WhileActiveContinous(InstantCommand(std::move(toRun), requirements));
}
@@ -87,6 +100,12 @@
Trigger Trigger::WhenInactive(std::function<void()> toRun,
std::initializer_list<Subsystem*> requirements) {
+ return WhenInactive(std::move(toRun), wpi::makeArrayRef(requirements.begin(),
+ requirements.end()));
+}
+
+Trigger Trigger::WhenInactive(std::function<void()> toRun,
+ wpi::ArrayRef<Subsystem*> requirements) {
return WhenInactive(InstantCommand(std::move(toRun), requirements));
}
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
index 4695c26..e50f09e 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,6 +8,7 @@
#pragma once
#include <functional>
+#include <initializer_list>
#include <memory>
#include <string>
@@ -132,7 +133,18 @@
*/
SequentialCommandGroup BeforeStarting(
std::function<void()> toRun,
- std::initializer_list<Subsystem*> requirements = {}) &&;
+ std::initializer_list<Subsystem*> requirements) &&;
+
+ /**
+ * 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,
+ wpi::ArrayRef<Subsystem*> requirements = {}) &&;
/**
* Decorates this command with a runnable to run after the command finishes.
@@ -143,7 +155,18 @@
*/
SequentialCommandGroup AndThen(
std::function<void()> toRun,
- std::initializer_list<Subsystem*> requirements = {}) &&;
+ 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,
+ wpi::ArrayRef<Subsystem*> requirements = {}) &&;
/**
* Decorates this command to run perpetually, ignoring its ordinary end
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h
index 81af2f7..481a51a 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -12,6 +12,7 @@
#include <frc/smartdashboard/Sendable.h>
#include <frc/smartdashboard/SendableHelper.h>
+#include <wpi/ArrayRef.h>
#include <wpi/SmallSet.h>
#include <wpi/Twine.h>
@@ -32,6 +33,13 @@
*/
void AddRequirements(std::initializer_list<Subsystem*> requirements);
+ /**
+ * Adds the specified requirements to the command.
+ *
+ * @param requirements the requirements to add
+ */
+ void AddRequirements(wpi::ArrayRef<Subsystem*> requirements);
+
void AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements);
wpi::SmallSet<Subsystem*, 4> GetRequirements() const override;
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
index c114d6a..f1d0917 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -157,8 +157,10 @@
void UnregisterSubsystem(Subsystem* subsystem);
void RegisterSubsystem(std::initializer_list<Subsystem*> subsystems);
+ void RegisterSubsystem(wpi::ArrayRef<Subsystem*> subsystems);
void UnregisterSubsystem(std::initializer_list<Subsystem*> subsystems);
+ void UnregisterSubsystem(wpi::ArrayRef<Subsystem*> subsystems);
/**
* Sets the default command for a subsystem. Registers that subsystem if it
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h
index f85f9be..eb88b5e 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,6 +8,9 @@
#pragma once
#include <functional>
+#include <initializer_list>
+
+#include <wpi/ArrayRef.h>
#include "frc2/command/CommandBase.h"
#include "frc2/command/CommandHelper.h"
@@ -36,7 +39,23 @@
std::function<void()> onExecute,
std::function<void(bool)> onEnd,
std::function<bool()> isFinished,
- std::initializer_list<Subsystem*> requirements = {});
+ std::initializer_list<Subsystem*> requirements);
+
+ /**
+ * Creates a new FunctionalCommand.
+ *
+ * @param onInit the function to run on command initialization
+ * @param onExecute the function to run on command execution
+ * @param onEnd the function to run on command end
+ * @param isFinished the function that determines whether the command has
+ * finished
+ * @param requirements the subsystems required by this command
+ */
+ FunctionalCommand(std::function<void()> onInit,
+ std::function<void()> onExecute,
+ std::function<void(bool)> onEnd,
+ std::function<bool()> isFinished,
+ wpi::ArrayRef<Subsystem*> requirements = {});
FunctionalCommand(FunctionalCommand&& other) = default;
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h
index 97b4da2..3f29b17 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,6 +10,8 @@
#include <functional>
#include <initializer_list>
+#include <wpi/ArrayRef.h>
+
#include "frc2/command/CommandBase.h"
#include "frc2/command/CommandHelper.h"
@@ -29,7 +31,17 @@
* @param requirements the subsystems required by this command
*/
InstantCommand(std::function<void()> toRun,
- std::initializer_list<Subsystem*> requirements = {});
+ std::initializer_list<Subsystem*> requirements);
+
+ /**
+ * Creates a new InstantCommand that runs the given Runnable with the given
+ * requirements.
+ *
+ * @param toRun the Runnable to run
+ * @param requirements the subsystems required by this command
+ */
+ InstantCommand(std::function<void()> toRun,
+ wpi::ArrayRef<Subsystem*> requirements = {});
InstantCommand(InstantCommand&& other) = default;
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h
index e5a22a5..f667e5f 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,6 +7,7 @@
#include <cmath>
#include <functional>
+#include <initializer_list>
#include <memory>
#include <frc/controller/PIDController.h>
@@ -18,6 +19,7 @@
#include <frc/kinematics/MecanumDriveWheelSpeeds.h>
#include <frc/trajectory/Trajectory.h>
#include <units/units.h>
+#include <wpi/ArrayRef.h>
#include "CommandBase.h"
#include "CommandHelper.h"
@@ -101,6 +103,57 @@
/**
* 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,
+ wpi::ArrayRef<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.
*
@@ -137,6 +190,44 @@
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,
+ wpi::ArrayRef<Subsystem*> requirements = {});
+
void Initialize() override;
void Execute() override;
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
index 847c693..59af028 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -12,6 +12,7 @@
#include <frc/Notifier.h>
#include <units/units.h>
+#include <wpi/ArrayRef.h>
#include "frc2/command/CommandBase.h"
#include "frc2/command/CommandHelper.h"
@@ -37,7 +38,17 @@
* @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);
+
+ /**
+ * Creates a new NotifierCommand.
+ *
+ * @param toRun the runnable for the notifier to run
+ * @param period the period at which the notifier should run
+ * @param requirements the subsystems required by this command
+ */
+ NotifierCommand(std::function<void()> toRun, units::second_t period,
+ wpi::ArrayRef<Subsystem*> requirements = {});
NotifierCommand(NotifierCommand&& other);
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
index 1089fd1..8653313 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -40,7 +40,23 @@
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
+ * PIDController.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param setpointSource the controller's reference (aka setpoint)
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ PIDCommand(PIDController controller,
+ std::function<double()> measurementSource,
+ std::function<double()> setpointSource,
+ std::function<void(double)> useOutput,
+ wpi::ArrayRef<Subsystem*> requirements = {});
/**
* Creates a new PIDCommand, which controls the given output with a
@@ -57,6 +73,21 @@
std::function<void(double)> useOutput,
std::initializer_list<Subsystem*> requirements);
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * PIDController with a constant setpoint.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param setpoint the controller's setpoint (aka setpoint)
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ PIDCommand(PIDController controller,
+ std::function<double()> measurementSource, double setpoint,
+ std::function<void(double)> useOutput,
+ wpi::ArrayRef<Subsystem*> requirements = {});
+
PIDCommand(PIDCommand&& other) = default;
PIDCommand(const PIDCommand& other) = default;
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
index 62389c3..3a6df88 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
@@ -24,8 +24,9 @@
* Creates a new PIDSubsystem.
*
* @param controller the PIDController to use
+ * @param initialPosition the initial setpoint of the subsystem
*/
- explicit PIDSubsystem(PIDController controller);
+ explicit PIDSubsystem(PIDController controller, double initialPosition = 0);
void Periodic() override;
@@ -62,7 +63,7 @@
protected:
PIDController m_controller;
- bool m_enabled;
+ bool m_enabled{false};
/**
* Returns the measurement of the process variable used by the PIDController.
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
index 49575de..b685230 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,6 +13,7 @@
#include <frc/controller/ProfiledPIDController.h>
#include <units/units.h>
+#include <wpi/ArrayRef.h>
#include "frc2/command/CommandBase.h"
#include "frc2/command/CommandHelper.h"
@@ -50,7 +51,29 @@
std::function<Distance_t()> measurementSource,
std::function<State()> goalSource,
std::function<void(double, State)> useOutput,
- std::initializer_list<Subsystem*> requirements = {})
+ 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<State()> goalSource,
+ std::function<void(double, State)> useOutput,
+ wpi::ArrayRef<Subsystem*> requirements = {})
: m_controller{controller},
m_measurement{std::move(measurementSource)},
m_goal{std::move(goalSource)},
@@ -81,6 +104,27 @@
/**
* 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,
+ wpi::ArrayRef<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.
@@ -107,6 +151,23 @@
* @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,
+ wpi::ArrayRef<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,
@@ -114,11 +175,29 @@
: 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,
+ wpi::ArrayRef<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 Initialize() override { m_controller.Reset(m_measurement()); }
void Execute() override {
m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
index d5302c5..919ed18 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -32,9 +32,13 @@
* Creates a new ProfiledPIDSubsystem.
*
* @param controller the ProfiledPIDController to use
+ * @param initialPosition the initial goal position of the subsystem
*/
- explicit ProfiledPIDSubsystem(frc::ProfiledPIDController<Distance> controller)
- : m_controller{controller} {}
+ explicit ProfiledPIDSubsystem(frc::ProfiledPIDController<Distance> controller,
+ Distance_t initialPosition = Distance_t{0})
+ : m_controller{controller} {
+ SetGoal(initialPosition);
+ }
void Periodic() override {
if (m_enabled) {
@@ -58,10 +62,10 @@
void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
/**
- * Enables the PID control. Resets the controller.
+ * Enables the PID control. Resets the controller.
*/
virtual void Enable() {
- m_controller.Reset();
+ m_controller.Reset(GetMeasurement());
m_enabled = true;
}
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
index 60d1b7e..580bc57 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -18,6 +18,7 @@
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/trajectory/Trajectory.h>
#include <units/units.h>
+#include <wpi/ArrayRef.h>
#include "frc2/Timer.h"
#include "frc2/command/CommandBase.h"
@@ -78,7 +79,44 @@
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
+ * provided trajectory. 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 feedforward A component for calculating the feedforward 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 output A function that consumes the computed left and right
+ * outputs (in volts) for the robot drive.
+ * @param requirements The subsystems to require.
+ */
+ RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::RamseteController controller,
+ frc::SimpleMotorFeedforward<units::meters> feedforward,
+ frc::DifferentialDriveKinematics kinematics,
+ std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
+ frc2::PIDController leftController,
+ frc2::PIDController rightController,
+ std::function<void(units::volt_t, units::volt_t)> output,
+ wpi::ArrayRef<Subsystem*> requirements = {});
/**
* Constructs a new RamseteCommand that, when executed, will follow the
@@ -104,6 +142,30 @@
output,
std::initializer_list<Subsystem*> requirements);
+ /**
+ * Constructs a new RamseteCommand that, when executed, will follow the
+ * provided trajectory. Performs no PID control and calculates no
+ * feedforwards; outputs are the raw wheel speeds from the RAMSETE controller,
+ * and will need to be converted into a usable form by the user.
+ *
+ * @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 kinematics The kinematics for the robot drivetrain.
+ * @param output A function that consumes the computed left and right
+ * wheel speeds.
+ * @param requirements The subsystems to require.
+ */
+ RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::RamseteController controller,
+ frc::DifferentialDriveKinematics kinematics,
+ std::function<void(units::meters_per_second_t,
+ units::meters_per_second_t)>
+ output,
+ wpi::ArrayRef<Subsystem*> requirements = {});
+
void Initialize() override;
void Execute() override;
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h
index aae7b74..2036230 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,6 +10,8 @@
#include <functional>
#include <initializer_list>
+#include <wpi/ArrayRef.h>
+
#include "frc2/command/CommandBase.h"
#include "frc2/command/CommandHelper.h"
@@ -30,7 +32,17 @@
* @param requirements the subsystems to require
*/
RunCommand(std::function<void()> toRun,
- std::initializer_list<Subsystem*> requirements = {});
+ std::initializer_list<Subsystem*> requirements);
+
+ /**
+ * Creates a new RunCommand. The Runnable will be run continuously until the
+ * command ends. Does not run when disabled.
+ *
+ * @param toRun the Runnable to run
+ * @param requirements the subsystems to require
+ */
+ RunCommand(std::function<void()> toRun,
+ wpi::ArrayRef<Subsystem*> requirements = {});
RunCommand(RunCommand&& other) = default;
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
index 32b7e76..c0c9096 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,6 +10,8 @@
#include <functional>
#include <initializer_list>
+#include <wpi/ArrayRef.h>
+
#include "frc2/command/CommandBase.h"
#include "frc2/command/CommandHelper.h"
@@ -32,7 +34,18 @@
* @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);
+
+ /**
+ * Creates a new StartEndCommand. Will run the given runnables when the
+ * command starts and when it ends.
+ *
+ * @param onInit the Runnable to run on command init
+ * @param onEnd the Runnable to run on command end
+ * @param requirements the subsystems required by this command
+ */
+ StartEndCommand(std::function<void()> onInit, std::function<void()> onEnd,
+ wpi::ArrayRef<Subsystem*> requirements = {});
StartEndCommand(StartEndCommand&& other) = default;
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
index ab3fe10..38ca60c 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,6 +7,7 @@
#include <cmath>
#include <functional>
+#include <initializer_list>
#include <memory>
#include <frc/controller/PIDController.h>
@@ -17,6 +18,7 @@
#include <frc/kinematics/SwerveModuleState.h>
#include <frc/trajectory/Trajectory.h>
#include <units/units.h>
+#include <wpi/ArrayRef.h>
#include "CommandBase.h"
#include "CommandHelper.h"
@@ -92,6 +94,42 @@
output,
std::initializer_list<Subsystem*> requirements);
+ /**
+ * 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,
+ wpi::ArrayRef<Subsystem*> requirements = {});
+
void Initialize() override;
void Execute() override;
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
index 7beafef..42d726d 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -32,6 +32,26 @@
}
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,
+ wpi::ArrayRef<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;
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
index e21de07..a83dc39 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,6 +11,7 @@
#include <initializer_list>
#include <frc/trajectory/TrapezoidProfile.h>
+#include <wpi/ArrayRef.h>
#include "frc2/Timer.h"
#include "frc2/command/CommandBase.h"
@@ -47,6 +48,20 @@
this->AddRequirements(requirements);
}
+ /**
+ * 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,
+ wpi::ArrayRef<Subsystem*> requirements = {})
+ : m_profile(profile), m_output(output) {
+ this->AddRequirements(requirements);
+ }
+
void Initialize() override {
m_timer.Reset();
m_timer.Start();
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
index a833f3c..c5d7e35 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
@@ -37,18 +37,21 @@
* 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)
+ explicit TrapezoidProfileSubsystem(Constraints constraints,
+ Distance_t initialPosition = Distance_t{0},
+ units::second_t period = 20_ms)
: m_constraints(constraints),
- m_state{position, Velocity_t(0)},
- m_goal{position, Velocity_t{0}},
+ m_state{initialPosition, Velocity_t(0)},
+ m_goal{initialPosition, 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);
+ if (m_enabled) {
+ UseState(m_state);
+ }
}
/**
@@ -74,10 +77,21 @@
*/
virtual void UseState(State state) = 0;
+ /**
+ * Enable the TrapezoidProfileSubsystem's output.
+ */
+ void Enable() { m_enabled = true; }
+
+ /**
+ * Disable the TrapezoidProfileSubsystem's output.
+ */
+ void Disable() { m_enabled = false; }
+
private:
Constraints m_constraints;
State m_state;
State m_goal;
units::second_t m_period;
+ bool m_enabled{false};
};
} // namespace frc2
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
index 6561652..af861d4 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
@@ -1,13 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
+
+#include <functional>
+#include <initializer_list>
#include <utility>
+#include <wpi/ArrayRef.h>
+
#include "Trigger.h"
namespace frc2 {
@@ -68,7 +73,16 @@
* @param requirements the required subsystems.
*/
Button WhenPressed(std::function<void()> toRun,
- std::initializer_list<Subsystem*> requirements = {});
+ std::initializer_list<Subsystem*> requirements);
+
+ /**
+ * 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,
+ wpi::ArrayRef<Subsystem*> requirements = {});
/**
* Binds a command to be started repeatedly while the button is pressed, and
@@ -105,7 +119,16 @@
* @param requirements the required subsystems.
*/
Button WhileHeld(std::function<void()> toRun,
- std::initializer_list<Subsystem*> requirements = {});
+ std::initializer_list<Subsystem*> requirements);
+
+ /**
+ * 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,
+ wpi::ArrayRef<Subsystem*> requirements = {});
/**
* Binds a command to be started when the button is pressed, and cancelled
@@ -170,7 +193,16 @@
* @param requirements the required subsystems.
*/
Button WhenReleased(std::function<void()> toRun,
- std::initializer_list<Subsystem*> requirements = {});
+ std::initializer_list<Subsystem*> requirements);
+
+ /**
+ * 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,
+ wpi::ArrayRef<Subsystem*> requirements = {});
/**
* Binds a command to start when the button is pressed, and be cancelled when
diff --git a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
index efc4bc0..7df6b4e 100644
--- a/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
+++ b/third_party/allwpilib_2019/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,10 +7,13 @@
#pragma once
-#include <atomic>
+#include <functional>
+#include <initializer_list>
#include <memory>
#include <utility>
+#include <wpi/ArrayRef.h>
+
#include "frc2/command/Command.h"
#include "frc2/command/CommandScheduler.h"
@@ -99,7 +102,16 @@
* @paaram requirements the required subsystems.
*/
Trigger WhenActive(std::function<void()> toRun,
- std::initializer_list<Subsystem*> requirements = {});
+ std::initializer_list<Subsystem*> requirements);
+
+ /**
+ * 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,
+ wpi::ArrayRef<Subsystem*> requirements = {});
/**
* Binds a command to be started repeatedly while the trigger is active, and
@@ -149,9 +161,17 @@
* @param toRun the runnable to execute.
* @param requirements the required subsystems.
*/
- Trigger WhileActiveContinous(
- std::function<void()> toRun,
- std::initializer_list<Subsystem*> requirements = {});
+ Trigger WhileActiveContinous(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements);
+
+ /**
+ * 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,
+ wpi::ArrayRef<Subsystem*> requirements = {});
/**
* Binds a command to be started when the trigger becomes active, and
@@ -242,7 +262,16 @@
* @param requirements the required subsystems.
*/
Trigger WhenInactive(std::function<void()> toRun,
- std::initializer_list<Subsystem*> requirements = {});
+ std::initializer_list<Subsystem*> requirements);
+
+ /**
+ * 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,
+ wpi::ArrayRef<Subsystem*> requirements = {});
/**
* Binds a command to start when the trigger becomes active, and be cancelled
diff --git a/third_party/allwpilib_2019/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java b/third_party/allwpilib_2019/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java
index 3c2858f..ac1107b 100644
--- a/third_party/allwpilib_2019/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java
+++ b/third_party/allwpilib_2019/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java
@@ -184,7 +184,6 @@
*/
public void addChild(String name, Sendable child) {
SendableRegistry.addLW(child, getSubsystem(), name);
- SendableRegistry.addChild(this, child);
}
/**
@@ -195,7 +194,6 @@
public void addChild(Sendable child) {
SendableRegistry.setSubsystem(child, getSubsystem());
SendableRegistry.enableLiveWindow(child);
- SendableRegistry.addChild(this, child);
}
/**
diff --git a/third_party/allwpilib_2019/wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp b/third_party/allwpilib_2019/wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp
index 6e665ea..3178455 100644
--- a/third_party/allwpilib_2019/wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp
+++ b/third_party/allwpilib_2019/wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp
@@ -101,7 +101,6 @@
void Subsystem::AddChild(const wpi::Twine& name, Sendable& child) {
auto& registry = SendableRegistry::GetInstance();
registry.AddLW(&child, registry.GetSubsystem(this), name);
- registry.AddChild(this, &child);
}
void Subsystem::AddChild(std::shared_ptr<Sendable> child) { AddChild(*child); }
@@ -112,7 +111,6 @@
auto& registry = SendableRegistry::GetInstance();
registry.SetSubsystem(&child, registry.GetSubsystem(this));
registry.EnableLiveWindow(&child);
- registry.AddChild(this, &child);
}
void Subsystem::ConfirmCommand() {
diff --git a/third_party/allwpilib_2019/wpilibc/build.gradle b/third_party/allwpilib_2019/wpilibc/build.gradle
index 8730824..4ee5766 100644
--- a/third_party/allwpilib_2019/wpilibc/build.gradle
+++ b/third_party/allwpilib_2019/wpilibc/build.gradle
@@ -231,6 +231,8 @@
}
}
+apply from: "${rootDir}/shared/cppDesktopTestTask.gradle"
+
tasks.withType(RunTestExecutable) {
args "--gtest_output=xml:test_detail.xml"
outputs.dir outputDir
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
index 728b1de..ea054ce 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
@@ -20,7 +20,9 @@
: m_dutyCycle{std::make_shared<DutyCycle>(
std::make_shared<DigitalInput>(channel))},
m_analogTrigger{m_dutyCycle.get()},
- m_counter{} {}
+ m_counter{} {
+ Init();
+}
DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle)
: m_dutyCycle{&dutyCycle, NullDeleter<DutyCycle>{}},
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/PWMTalonFX.cpp b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/PWMTalonFX.cpp
new file mode 100644
index 0000000..09b7163
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/PWMTalonFX.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PWMTalonFX.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+PWMTalonFX::PWMTalonFX(int channel) : PWMSpeedController(channel) {
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_TalonFX, GetChannel() + 1);
+ SendableRegistry::GetInstance().SetName(this, "PWMTalonFX", GetChannel());
+}
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/PWMVenom.cpp b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/PWMVenom.cpp
new file mode 100644
index 0000000..9fa14b7
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/PWMVenom.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PWMVenom.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+PWMVenom::PWMVenom(int channel) : PWMSpeedController(channel) {
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_FusionVenom, GetChannel() + 1);
+ SendableRegistry::GetInstance().SetName(this, "PWMVenom", GetChannel());
+}
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/SPI.cpp b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/SPI.cpp
index 074cc79..d51fa3b 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/SPI.cpp
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/SPI.cpp
@@ -311,6 +311,14 @@
return val;
}
+void SPI::ConfigureAutoStall(HAL_SPIPort port, int csToSclkTicks,
+ int stallTicks, int pow2BytesPerRead) {
+ int32_t status = 0;
+ HAL_ConfigureSPIAutoStall(m_port, csToSclkTicks, stallTicks, pow2BytesPerRead,
+ &status);
+ wpi_setHALError(status);
+}
+
void SPI::InitAccumulator(units::second_t period, int cmd, int xferSize,
int validMask, int validValue, int dataShift,
int dataSize, bool isSigned, bool bigEndian) {
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/controller/PIDController.cpp
index 264859f..3abd4ba 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/controller/PIDController.cpp
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/controller/PIDController.cpp
@@ -22,7 +22,7 @@
: m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
static int instances = 0;
instances++;
- HAL_Report(HALUsageReporting::kResourceType_PIDController, instances);
+ HAL_Report(HALUsageReporting::kResourceType_PIDController2, instances);
frc::SendableRegistry::GetInstance().Add(this, "PIDController", instances);
}
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
new file mode 100644
index 0000000..a0fffc6
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/controller/ProfiledPIDController.h"
+
+#include <hal/FRCUsageReporting.h>
+
+void frc::detail::ReportProfiledPIDController() {
+ static int instances = 0;
+ ++instances;
+ HAL_Report(HALUsageReporting::kResourceType_ProfiledPIDController, instances);
+}
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
index 8591da4..07a8e3b 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
@@ -7,6 +7,8 @@
#include "frc/kinematics/DifferentialDriveOdometry.h"
+#include <hal/FRCUsageReporting.h>
+
using namespace frc;
DifferentialDriveOdometry::DifferentialDriveOdometry(
@@ -14,6 +16,8 @@
: m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
+ HAL_Report(HALUsageReporting::kResourceType_Odometry,
+ HALUsageReporting::kOdometry_DifferentialDrive);
}
const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
index 615635a..3843483 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
@@ -7,6 +7,8 @@
#include "frc/kinematics/MecanumDriveOdometry.h"
+#include <hal/FRCUsageReporting.h>
+
using namespace frc;
MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
@@ -15,6 +17,8 @@
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
+ HAL_Report(HALUsageReporting::kResourceType_Odometry,
+ HALUsageReporting::kOdometry_MecanumDrive);
}
const Pose2d& MecanumDriveOdometry::UpdateWithTime(
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
index bce6196..9d57ad6 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
@@ -134,6 +134,12 @@
comp.subsystem = subsystem.str();
}
+void SendableRegistry::AddChild(Sendable* parent, Sendable* child) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(child);
+ comp.parent = parent;
+}
+
void SendableRegistry::AddChild(Sendable* parent, void* child) {
std::scoped_lock lock(m_impl->mutex);
auto& comp = m_impl->GetOrAdd(child);
@@ -147,6 +153,10 @@
UID compUid = it->getSecond();
m_impl->components.erase(compUid - 1);
m_impl->componentMap.erase(it);
+ // update any parent pointers
+ for (auto&& comp : m_impl->components) {
+ if (comp->parent == sendable) comp->parent = nullptr;
+ }
return true;
}
@@ -164,6 +174,10 @@
comp.builder.ClearProperties();
to->InitSendable(comp.builder);
}
+ // update any parent pointers
+ for (auto&& comp : m_impl->components) {
+ if (comp->parent == from) comp->parent = to;
+ }
}
bool SendableRegistry::Contains(const Sendable* sendable) const {
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
index 7b2b2b6..84d4f37 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
@@ -107,6 +107,33 @@
(t - prevSample.t) / (sample.t - prevSample.t));
}
+Trajectory Trajectory::TransformBy(const Transform2d& transform) {
+ auto& firstState = m_states[0];
+ auto& firstPose = firstState.pose;
+
+ // Calculate the transformed first pose.
+ auto newFirstPose = firstPose + transform;
+ auto newStates = m_states;
+ newStates[0].pose = newFirstPose;
+
+ for (unsigned int i = 1; i < newStates.size(); i++) {
+ auto& state = newStates[i];
+ // We are transforming relative to the coordinate frame of the new initial
+ // pose.
+ state.pose = newFirstPose + (state.pose - firstPose);
+ }
+
+ return Trajectory(newStates);
+}
+
+Trajectory Trajectory::RelativeTo(const Pose2d& pose) {
+ auto newStates = m_states;
+ for (auto& state : newStates) {
+ state.pose = state.pose.RelativeTo(pose);
+ }
+ return Trajectory(newStates);
+}
+
void frc::to_json(wpi::json& json, const Trajectory::State& state) {
json = wpi::json{{"time", state.t.to<double>()},
{"velocity", state.velocity.to<double>()},
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
index 92c21f6..3c95472 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,11 +9,16 @@
#include <utility>
+#include "frc/DriverStation.h"
#include "frc/spline/SplineHelper.h"
+#include "frc/spline/SplineParameterizer.h"
#include "frc/trajectory/TrajectoryParameterizer.h"
using namespace frc;
+const Trajectory TrajectoryGenerator::kDoNothingTrajectory(
+ std::vector<Trajectory::State>{Trajectory::State()});
+
Trajectory TrajectoryGenerator::GenerateTrajectory(
Spline<3>::ControlVector initial,
const std::vector<Translation2d>& interiorWaypoints,
@@ -29,9 +34,15 @@
end.y[1] *= -1;
}
- auto points =
- SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors(
- initial, interiorWaypoints, end));
+ std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
+ try {
+ points =
+ SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors(
+ initial, interiorWaypoints, end));
+ } catch (SplineParameterizer::MalformedSplineException& e) {
+ DriverStation::ReportError(e.what());
+ return kDoNothingTrajectory;
+ }
// After trajectory generation, flip theta back so it's relative to the
// field. Also fix curvature.
@@ -68,8 +79,14 @@
}
}
- auto points = SplinePointsFromSplines(
- SplineHelper::QuinticSplinesFromControlVectors(controlVectors));
+ std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
+ try {
+ points = SplinePointsFromSplines(
+ SplineHelper::QuinticSplinesFromControlVectors(controlVectors));
+ } catch (SplineParameterizer::MalformedSplineException& e) {
+ DriverStation::ReportError(e.what());
+ return kDoNothingTrajectory;
+ }
// After trajectory generation, flip theta back so it's relative to the
// field. Also fix curvature.
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/cppcs/RobotBase.cpp b/third_party/allwpilib_2019/wpilibc/src/main/native/cppcs/RobotBase.cpp
index 5fd824d..eaf4ddc 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/cppcs/RobotBase.cpp
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/cppcs/RobotBase.cpp
@@ -36,7 +36,7 @@
return -1;
}
HAL_Report(HALUsageReporting::kResourceType_Language,
- HALUsageReporting::kLanguage_CPlusPlus);
+ HALUsageReporting::kLanguage_CPlusPlus, 0, GetWPILibVersion());
wpi::outs() << "\n********** Robot program starting **********\n";
return 0;
}
@@ -124,7 +124,11 @@
auto inst = nt::NetworkTableInstance::GetDefault();
inst.SetNetworkIdentity("Robot");
+#ifdef __FRC_ROBORIO__
inst.StartServer("/home/lvuser/networktables.ini");
+#else
+ inst.StartServer();
+#endif
SmartDashboard::init();
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/AddressableLED.h b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/AddressableLED.h
index 2ffe0ea..5534ae4 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/AddressableLED.h
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/AddressableLED.h
@@ -15,6 +15,8 @@
#include <wpi/ArrayRef.h>
#include "frc/ErrorBase.h"
+#include "util/Color.h"
+#include "util/Color8Bit.h"
namespace frc {
@@ -54,6 +56,28 @@
* @param v the v value [0-255]
*/
void SetHSV(int h, int s, int v);
+
+ /*
+ * Sets a specific LED in the buffer.
+ *
+ * @param color The color of the LED
+ */
+ void SetLED(const Color& color) {
+ this->r = color.red * 255;
+ this->g = color.green * 255;
+ this->b = color.blue * 255;
+ }
+
+ /*
+ * Sets a specific LED in the buffer.
+ *
+ * @param color The color of the LED
+ */
+ void SetLED(const Color8Bit& color) {
+ this->r = color.red;
+ this->g = color.green;
+ this->b = color.blue;
+ }
};
/**
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/PWMTalonFX.h b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/PWMTalonFX.h
new file mode 100644
index 0000000..d85c7ca
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/PWMTalonFX.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 "frc/PWMSpeedController.h"
+
+namespace frc {
+
+/**
+ * Cross the Road Electronics (CTRE) Talon FX Speed Controller with PWM
+ * control.
+ *
+ * Note that the Talon FX 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 FX 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 PWMTalonFX : public PWMSpeedController {
+ public:
+ /**
+ * Construct a Talon FX connected via PWM.
+ *
+ * @param channel The PWM channel that the Talon FX is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+ explicit PWMTalonFX(int channel);
+
+ PWMTalonFX(PWMTalonFX&&) = default;
+ PWMTalonFX& operator=(PWMTalonFX&&) = default;
+};
+
+} // namespace frc
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/PWMVenom.h b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/PWMVenom.h
new file mode 100644
index 0000000..189db43
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/PWMVenom.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 "frc/PWMSpeedController.h"
+
+namespace frc {
+
+/**
+ * Playing with Fusion Venom Smart Motor with PWM control.
+ *
+ * Note that the Venom uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended.
+ *
+ * \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 PWMVenom : public PWMSpeedController {
+ public:
+ /**
+ * Construct a Venom connected via PWM.
+ *
+ * @param channel The PWM channel that the Venom is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+ explicit PWMVenom(int channel);
+
+ PWMVenom(PWMVenom&&) = default;
+ PWMVenom& operator=(PWMVenom&&) = default;
+};
+
+} // namespace frc
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/SPI.h b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/SPI.h
index fb4835e..8e721bc 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/SPI.h
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/SPI.h
@@ -270,6 +270,19 @@
int GetAutoDroppedCount();
/**
+ * Configure the Auto SPI Stall time between reads.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
+ * MXP.
+ * @param csToSclkTicks the number of ticks to wait before asserting the cs
+ * pin
+ * @param stallTicks the number of ticks to stall for
+ * @param pow2BytesPerRead the number of bytes to read before stalling
+ */
+ void ConfigureAutoStall(HAL_SPIPort port, int csToSclkTicks, int stallTicks,
+ int pow2BytesPerRead);
+
+ /**
* Initialize the accumulator.
*
* @param period Time between reads
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/SlewRateLimiter.h b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/SlewRateLimiter.h
new file mode 100644
index 0000000..7236a30
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/SlewRateLimiter.h
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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/Timer.h>
+
+#include <algorithm>
+
+#include <units/units.h>
+
+namespace frc {
+/**
+ * A class that limits the rate of change of an input value. Useful for
+ * implementing voltage, setpoint, and/or output ramps. A slew-rate limit
+ * is most appropriate when the quantity being controlled is a velocity or
+ * a voltage; when controlling a position, consider using a TrapezoidProfile
+ * instead.
+ *
+ * @see TrapezoidProfile
+ */
+template <class Unit>
+class SlewRateLimiter {
+ using Unit_t = units::unit_t<Unit>;
+ using Rate = units::compound_unit<Unit, units::inverse<units::seconds>>;
+ using Rate_t = units::unit_t<Rate>;
+
+ public:
+ /**
+ * Creates a new SlewRateLimiter with the given rate limit and initial value.
+ *
+ * @param rateLimit The rate-of-change limit.
+ * @param initialValue The initial value of the input.
+ */
+ explicit SlewRateLimiter(Rate_t rateLimit, Unit_t initialValue = Unit_t{0})
+ : m_rateLimit{rateLimit}, m_prevVal{initialValue} {
+ m_timer.Start();
+ }
+
+ /**
+ * Filters the input to limit its slew rate.
+ *
+ * @param input The input value whose slew rate is to be limited.
+ * @return The filtered value, which will not change faster than the slew
+ * rate.
+ */
+ Unit_t Calculate(Unit_t input) {
+ m_prevVal += std::clamp(input - m_prevVal, -m_rateLimit * m_timer.Get(),
+ m_rateLimit * m_timer.Get());
+ m_timer.Reset();
+ return m_prevVal;
+ }
+
+ /**
+ * Resets the slew rate limiter to the specified value; ignores the rate limit
+ * when doing so.
+ *
+ * @param value The value to reset to.
+ */
+ void Reset(Unit_t value) {
+ m_timer.Reset();
+ m_prevVal = value;
+ }
+
+ private:
+ frc2::Timer m_timer;
+ Rate_t m_rateLimit;
+ Unit_t m_prevVal;
+};
+} // namespace frc
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
index 96fd331..079d96e 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -21,6 +21,9 @@
#include "frc/trajectory/TrapezoidProfile.h"
namespace frc {
+namespace detail {
+void ReportProfiledPIDController();
+} // namespace detail
/**
* Implements a PID control loop whose setpoint is constrained by a trapezoid
@@ -43,7 +46,8 @@
public:
/**
* Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
- * Kd.
+ * Kd. Users should call reset() when they first start running the controller
+ * to avoid unwanted behavior.
*
* @param Kp The proportional coefficient.
* @param Ki The integral coefficient.
@@ -54,7 +58,9 @@
*/
ProfiledPIDController(double Kp, double Ki, double Kd,
Constraints constraints, units::second_t period = 20_ms)
- : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}
+ : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
+ detail::ReportProfiledPIDController();
+ }
~ProfiledPIDController() override = default;
@@ -287,9 +293,34 @@
}
/**
- * Reset the previous error, the integral term, and disable the controller.
+ * Reset the previous error and the integral term.
+ *
+ * @param measurement The current measured State of the system.
*/
- void Reset() { m_controller.Reset(); }
+ void Reset(const State& measurement) {
+ m_controller.Reset();
+ m_setpoint = measurement;
+ }
+
+ /**
+ * Reset the previous error and the integral term.
+ *
+ * @param measuredPosition The current measured position of the system.
+ * @param measuredVelocity The current measured velocity of the system.
+ */
+ void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity) {
+ Reset(State{measuredPosition, measuredVelocity});
+ }
+
+ /**
+ * Reset the previous error and the integral term.
+ *
+ * @param measuredPosition The current measured position of the system. The
+ * velocity is assumed to be zero.
+ */
+ void Reset(Distance_t measuredPosition) {
+ Reset(measuredPosition, Velocity_t(0));
+ }
void InitSendable(frc::SendableBuilder& builder) override {
builder.SetSmartDashboardType("ProfiledPIDController");
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
index cb2121f..e986029 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
@@ -7,6 +7,7 @@
#pragma once
+#include <hal/FRCUsageReporting.h>
#include <units/units.h>
#include "frc/kinematics/ChassisSpeeds.h"
@@ -31,8 +32,11 @@
* empirical value may be larger than the physical measured value due to
* scrubbing effects.
*/
- constexpr explicit DifferentialDriveKinematics(units::meter_t trackWidth)
- : trackWidth(trackWidth) {}
+ explicit DifferentialDriveKinematics(units::meter_t trackWidth)
+ : trackWidth(trackWidth) {
+ HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+ HALUsageReporting::kKinematics_DifferentialDrive);
+ }
/**
* Returns a chassis speed from left and right component velocities using
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
index 6b0329d..47b1b34 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
@@ -9,6 +9,7 @@
#include <Eigen/Core>
#include <Eigen/QR>
+#include <hal/FRCUsageReporting.h>
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
@@ -63,6 +64,8 @@
SetInverseKinematics(frontLeftWheel, frontRightWheel, rearLeftWheel,
rearRightWheel);
m_forwardKinematics = m_inverseKinematics.householderQr();
+ HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+ HALUsageReporting::kKinematics_MecanumDrive);
}
MecanumDriveKinematics(const MecanumDriveKinematics&) = default;
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
index 1bdbcea..f889363 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
@@ -12,6 +12,7 @@
#include <Eigen/Core>
#include <Eigen/QR>
+#include <hal/FRCUsageReporting.h>
#include <units/units.h>
#include "frc/geometry/Rotation2d.h"
@@ -71,6 +72,9 @@
}
m_forwardKinematics = m_inverseKinematics.householderQr();
+
+ HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+ HALUsageReporting::kKinematics_SwerveDrive);
}
SwerveDriveKinematics(const SwerveDriveKinematics&) = default;
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
index 23852d0..e794388 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
@@ -7,6 +7,8 @@
#pragma once
+#include <hal/FRCUsageReporting.h>
+
namespace frc {
template <size_t NumModules>
SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
@@ -15,6 +17,8 @@
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
+ HAL_Report(HALUsageReporting::kResourceType_Odometry,
+ HALUsageReporting::kOdometry_SwerveDrive);
}
template <size_t NumModules>
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/smartdashboard/SendableRegistry.h b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/smartdashboard/SendableRegistry.h
index 53dbba3..78dabc6 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/smartdashboard/SendableRegistry.h
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/smartdashboard/SendableRegistry.h
@@ -125,6 +125,15 @@
* @param parent parent object
* @param child child object
*/
+ void AddChild(Sendable* parent, Sendable* child);
+
+ /**
+ * Adds a child object to an object. Adds the child object to the registry
+ * if it's not already present.
+ *
+ * @param parent parent object
+ * @param child child object
+ */
void AddChild(Sendable* parent, void* child);
/**
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h
index 34cf026..d3a5e6e 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -33,10 +33,13 @@
#include <frc/spline/Spline.h>
+#include <stack>
+#include <string>
#include <utility>
#include <vector>
#include <units/units.h>
+#include <wpi/Twine.h>
namespace frc {
@@ -47,6 +50,11 @@
public:
using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
+ struct MalformedSplineException : public std::runtime_error {
+ explicit MalformedSplineException(const char* what_arg)
+ : runtime_error(what_arg) {}
+ };
+
/**
* Parameterizes the spline. This method breaks up the spline into various
* arcs until their dx, dy, and dtheta are within specific tolerances.
@@ -64,14 +72,48 @@
static std::vector<PoseWithCurvature> Parameterize(const Spline<Dim>& spline,
double t0 = 0.0,
double t1 = 1.0) {
- std::vector<PoseWithCurvature> arr;
+ std::vector<PoseWithCurvature> splinePoints;
- // The parameterization does not add the first initial point. Let's add
- // that.
- arr.push_back(spline.GetPoint(t0));
+ // The parameterization does not add the initial point. Let's add that.
+ splinePoints.push_back(spline.GetPoint(t0));
- GetSegmentArc(spline, &arr, t0, t1);
- return arr;
+ // We use an "explicit stack" to simulate recursion, instead of a recursive
+ // function call This give us greater control, instead of a stack overflow
+ std::stack<StackContents> stack;
+ stack.emplace(StackContents{t0, t1});
+
+ StackContents current;
+ PoseWithCurvature start;
+ PoseWithCurvature end;
+ int iterations = 0;
+
+ while (!stack.empty()) {
+ current = stack.top();
+ stack.pop();
+ start = spline.GetPoint(current.t0);
+ end = spline.GetPoint(current.t1);
+
+ const auto twist = start.first.Log(end.first);
+
+ if (units::math::abs(twist.dy) > kMaxDy ||
+ units::math::abs(twist.dx) > kMaxDx ||
+ units::math::abs(twist.dtheta) > kMaxDtheta) {
+ stack.emplace(StackContents{(current.t0 + current.t1) / 2, current.t1});
+ stack.emplace(StackContents{current.t0, (current.t0 + current.t1) / 2});
+ } else {
+ splinePoints.push_back(spline.GetPoint(current.t1));
+ }
+
+ if (iterations++ >= kMaxIterations) {
+ throw MalformedSplineException(
+ "Could not parameterize a malformed spline. "
+ "This means that you probably had two or more adjacent "
+ "waypoints that were very close together with headings "
+ "in opposing directions.");
+ }
+ }
+
+ return splinePoints;
}
private:
@@ -80,33 +122,19 @@
static constexpr units::meter_t kMaxDy = 0.05_in;
static constexpr units::radian_t kMaxDtheta = 0.0872_rad;
+ struct StackContents {
+ double t0;
+ double t1;
+ };
+
/**
- * Breaks up the spline into arcs until the dx, dy, and theta of each arc is
- * within tolerance.
- *
- * @param spline The spline to parameterize.
- * @param vector Pointer to vector of poses.
- * @param t0 Starting point for arc.
- * @param t1 Ending point for arc.
+ * A malformed spline does not actually explode the LIFO stack size. Instead,
+ * the stack size stays at a relatively small number (e.g. 30) and never
+ * decreases. Because of this, we must count iterations. Even long, complex
+ * paths don't usually go over 300 iterations, so hitting this maximum should
+ * definitely indicate something has gone wrong.
*/
- template <int Dim>
- static void GetSegmentArc(const Spline<Dim>& spline,
- std::vector<PoseWithCurvature>* vector, double t0,
- double t1) {
- const auto start = spline.GetPoint(t0);
- const auto end = spline.GetPoint(t1);
-
- const auto twist = start.first.Log(end.first);
-
- if (units::math::abs(twist.dy) > kMaxDy ||
- units::math::abs(twist.dx) > kMaxDx ||
- units::math::abs(twist.dtheta) > kMaxDtheta) {
- GetSegmentArc(spline, vector, t0, (t0 + t1) / 2);
- GetSegmentArc(spline, vector, (t0 + t1) / 2, t1);
- } else {
- vector->push_back(spline.GetPoint(t1));
- }
- }
+ static constexpr int kMaxIterations = 5000;
friend class CubicHermiteSplineTest;
friend class QuinticHermiteSplineTest;
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
index 96c2c6e..13e32d6 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
@@ -12,6 +12,7 @@
#include <units/units.h>
#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Transform2d.h"
namespace wpi {
class json;
@@ -105,6 +106,34 @@
*/
State Sample(units::second_t t) const;
+ /**
+ * Transforms all poses in the trajectory by the given transform. This is
+ * useful for converting a robot-relative trajectory into a field-relative
+ * trajectory. This works with respect to the first pose in the trajectory.
+ *
+ * @param transform The transform to transform the trajectory by.
+ * @return The transformed trajectory.
+ */
+ Trajectory TransformBy(const Transform2d& transform);
+
+ /**
+ * Transforms all poses in the trajectory so that they are relative to the
+ * given pose. This is useful for converting a field-relative trajectory
+ * into a robot-relative trajectory.
+ *
+ * @param pose The pose that is the origin of the coordinate frame that
+ * the current trajectory will be transformed into.
+ * @return The transformed trajectory.
+ */
+ Trajectory RelativeTo(const Pose2d& pose);
+
+ /**
+ * Returns the initial pose of the trajectory.
+ *
+ * @return The initial pose of the trajectory.
+ */
+ Pose2d InitialPose() const { return Sample(0_s).pose; }
+
private:
std::vector<State> m_states;
units::second_t m_totalTime;
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
index 4d6eff6..40e7e7b 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -113,5 +113,8 @@
}
return splinePoints;
}
+
+ private:
+ static const Trajectory kDoNothingTrajectory;
};
} // namespace frc
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
index 0479cfe..575088e 100644
--- a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -7,6 +7,7 @@
#pragma once
+#include <hal/FRCUsageReporting.h>
#include <units/units.h>
namespace frc {
@@ -53,6 +54,13 @@
public:
class Constraints {
public:
+ Constraints() {
+ HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, 1);
+ }
+ Constraints(Velocity_t maxVelocity_, Acceleration_t maxAcceleration_)
+ : maxVelocity{maxVelocity_}, maxAcceleration{maxAcceleration_} {
+ HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, 1);
+ }
Velocity_t maxVelocity{0};
Acceleration_t maxAcceleration{0};
};
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/util/Color.h b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/util/Color.h
new file mode 100644
index 0000000..2538d60
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/util/Color.h
@@ -0,0 +1,949 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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>
+
+namespace frc {
+
+/**
+ * Represents colors that can be used with Addressable LEDs.
+ *
+ * Limited to 12 bits of precision.
+ */
+class Color {
+ public:
+ /*
+ * FIRST Colors
+ */
+
+ /**
+ * #1560BD.
+ */
+ static const Color kDenim;
+
+ /**
+ * #0066B3.
+ */
+ static const Color kFirstBlue;
+
+ /**
+ * #ED1C24.
+ */
+ static const Color kFirstRed;
+
+ /*
+ * Standard Colors
+ */
+
+ /**
+ * #F0F8FF.
+ */
+ static const Color kAliceBlue;
+
+ /**
+ * #FAEBD7.
+ */
+ static const Color kAntiqueWhite;
+
+ /**
+ * #00FFFF.
+ */
+ static const Color kAqua;
+
+ /**
+ * #7FFFD4.
+ */
+ static const Color kAquamarine;
+
+ /**
+ * #F0FFFF.
+ */
+ static const Color kAzure;
+
+ /**
+ * #F5F5DC.
+ */
+ static const Color kBeige;
+
+ /**
+ * #FFE4C4.
+ */
+ static const Color kBisque;
+
+ /**
+ * #000000.
+ */
+ static const Color kBlack;
+
+ /**
+ * #FFEBCD.
+ */
+ static const Color kBlanchedAlmond;
+
+ /**
+ * #0000FF.
+ */
+ static const Color kBlue;
+
+ /**
+ * #8A2BE2.
+ */
+ static const Color kBlueViolet;
+
+ /**
+ * #A52A2A.
+ */
+ static const Color kBrown;
+
+ /**
+ * #DEB887.
+ */
+ static const Color kBurlywood;
+
+ /**
+ * #5F9EA0.
+ */
+ static const Color kCadetBlue;
+
+ /**
+ * #7FFF00.
+ */
+ static const Color kChartreuse;
+
+ /**
+ * #D2691E.
+ */
+ static const Color kChocolate;
+
+ /**
+ * #FF7F50.
+ */
+ static const Color kCoral;
+
+ /**
+ * #6495ED.
+ */
+ static const Color kCornflowerBlue;
+
+ /**
+ * #FFF8DC.
+ */
+ static const Color kCornsilk;
+
+ /**
+ * #DC143C.
+ */
+ static const Color kCrimson;
+
+ /**
+ * #00FFFF.
+ */
+ static const Color kCyan;
+
+ /**
+ * #00008B.
+ */
+ static const Color kDarkBlue;
+
+ /**
+ * #008B8B.
+ */
+ static const Color kDarkCyan;
+
+ /**
+ * #B8860B.
+ */
+ static const Color kDarkGoldenrod;
+
+ /**
+ * #A9A9A9.
+ */
+ static const Color kDarkGray;
+
+ /**
+ * #006400.
+ */
+ static const Color kDarkGreen;
+
+ /**
+ * #BDB76B.
+ */
+ static const Color kDarkKhaki;
+
+ /**
+ * #8B008B.
+ */
+ static const Color kDarkMagenta;
+
+ /**
+ * #556B2F.
+ */
+ static const Color kDarkOliveGreen;
+
+ /**
+ * #FF8C00.
+ */
+ static const Color kDarkOrange;
+
+ /**
+ * #9932CC.
+ */
+ static const Color kDarkOrchid;
+
+ /**
+ * #8B0000.
+ */
+ static const Color kDarkRed;
+
+ /**
+ * #E9967A.
+ */
+ static const Color kDarkSalmon;
+
+ /**
+ * #8FBC8F.
+ */
+ static const Color kDarkSeaGreen;
+
+ /**
+ * #483D8B.
+ */
+ static const Color kDarkSlateBlue;
+
+ /**
+ * #2F4F4F.
+ */
+ static const Color kDarkSlateGray;
+
+ /**
+ * #00CED1.
+ */
+ static const Color kDarkTurquoise;
+
+ /**
+ * #9400D3.
+ */
+ static const Color kDarkViolet;
+
+ /**
+ * #FF1493.
+ */
+ static const Color kDeepPink;
+
+ /**
+ * #00BFFF.
+ */
+ static const Color kDeepSkyBlue;
+
+ /**
+ * #696969.
+ */
+ static const Color kDimGray;
+
+ /**
+ * #1E90FF.
+ */
+ static const Color kDodgerBlue;
+
+ /**
+ * #B22222.
+ */
+ static const Color kFirebrick;
+
+ /**
+ * #FFFAF0.
+ */
+ static const Color kFloralWhite;
+
+ /**
+ * #228B22.
+ */
+ static const Color kForestGreen;
+
+ /**
+ * #FF00FF.
+ */
+ static const Color kFuchsia;
+
+ /**
+ * #DCDCDC.
+ */
+ static const Color kGainsboro;
+
+ /**
+ * #F8F8FF.
+ */
+ static const Color kGhostWhite;
+
+ /**
+ * #FFD700.
+ */
+ static const Color kGold;
+
+ /**
+ * #DAA520.
+ */
+ static const Color kGoldenrod;
+
+ /**
+ * #808080.
+ */
+ static const Color kGray;
+
+ /**
+ * #008000.
+ */
+ static const Color kGreen;
+
+ /**
+ * #ADFF2F.
+ */
+ static const Color kGreenYellow;
+
+ /**
+ * #F0FFF0.
+ */
+ static const Color kHoneydew;
+
+ /**
+ * #FF69B4.
+ */
+ static const Color kHotPink;
+
+ /**
+ * #CD5C5C.
+ */
+ static const Color kIndianRed;
+
+ /**
+ * #4B0082.
+ */
+ static const Color kIndigo;
+
+ /**
+ * #FFFFF0.
+ */
+ static const Color kIvory;
+
+ /**
+ * #F0E68C.
+ */
+ static const Color kKhaki;
+
+ /**
+ * #E6E6FA.
+ */
+ static const Color kLavender;
+
+ /**
+ * #FFF0F5.
+ */
+ static const Color kLavenderBlush;
+
+ /**
+ * #7CFC00.
+ */
+ static const Color kLawnGreen;
+
+ /**
+ * #FFFACD.
+ */
+ static const Color kLemonChiffon;
+
+ /**
+ * #ADD8E6.
+ */
+ static const Color kLightBlue;
+
+ /**
+ * #F08080.
+ */
+ static const Color kLightCoral;
+
+ /**
+ * #E0FFFF.
+ */
+ static const Color kLightCyan;
+
+ /**
+ * #FAFAD2.
+ */
+ static const Color kLightGoldenrodYellow;
+
+ /**
+ * #D3D3D3.
+ */
+ static const Color kLightGray;
+
+ /**
+ * #90EE90.
+ */
+ static const Color kLightGreen;
+
+ /**
+ * #FFB6C1.
+ */
+ static const Color kLightPink;
+
+ /**
+ * #FFA07A.
+ */
+ static const Color kLightSalmon;
+
+ /**
+ * #20B2AA.
+ */
+ static const Color kLightSeagGeen;
+
+ /**
+ * #87CEFA.
+ */
+ static const Color kLightSkyBlue;
+
+ /**
+ * #778899.
+ */
+ static const Color kLightSlateGray;
+
+ /**
+ * #B0C4DE.
+ */
+ static const Color kLightSteellue;
+
+ /**
+ * #FFFFE0.
+ */
+ static const Color kLightYellow;
+
+ /**
+ * #00FF00.
+ */
+ static const Color kLime;
+
+ /**
+ * #32CD32.
+ */
+ static const Color kLimeGreen;
+
+ /**
+ * #FAF0E6.
+ */
+ static const Color kLinen;
+
+ /**
+ * #FF00FF.
+ */
+ static const Color kMagenta;
+
+ /**
+ * #800000.
+ */
+ static const Color kMaroon;
+
+ /**
+ * #66CDAA.
+ */
+ static const Color kMediumAquamarine;
+
+ /**
+ * #0000CD.
+ */
+ static const Color kMediumBlue;
+
+ /**
+ * #BA55D3.
+ */
+ static const Color kMediumOrchid;
+
+ /**
+ * #9370DB.
+ */
+ static const Color kMediumPurple;
+
+ /**
+ * #3CB371.
+ */
+ static const Color kMediumSeaGreen;
+
+ /**
+ * #7B68EE.
+ */
+ static const Color kMediumSlateBlue;
+
+ /**
+ * #00FA9A.
+ */
+ static const Color kMediumSpringGreen;
+
+ /**
+ * #48D1CC.
+ */
+ static const Color kMediumTurquoise;
+
+ /**
+ * #C71585.
+ */
+ static const Color kMediumVioletRed;
+
+ /**
+ * #191970.
+ */
+ static const Color kMidnightBlue;
+
+ /**
+ * #F5FFFA.
+ */
+ static const Color kMintcream;
+
+ /**
+ * #FFE4E1.
+ */
+ static const Color kMistyRose;
+
+ /**
+ * #FFE4B5.
+ */
+ static const Color kMoccasin;
+
+ /**
+ * #FFDEAD.
+ */
+ static const Color kNavajoWhite;
+
+ /**
+ * #000080.
+ */
+ static const Color kNavy;
+
+ /**
+ * #FDF5E6.
+ */
+ static const Color kOldLace;
+
+ /**
+ * #808000.
+ */
+ static const Color kOlive;
+
+ /**
+ * #6B8E23.
+ */
+ static const Color kOliveDrab;
+
+ /**
+ * #FFA500.
+ */
+ static const Color kOrange;
+
+ /**
+ * #FF4500.
+ */
+ static const Color kOrangeRed;
+
+ /**
+ * #DA70D6.
+ */
+ static const Color kOrchid;
+
+ /**
+ * #EEE8AA.
+ */
+ static const Color kPaleGoldenrod;
+
+ /**
+ * #98FB98.
+ */
+ static const Color kPaleGreen;
+
+ /**
+ * #AFEEEE.
+ */
+ static const Color kPaleTurquoise;
+
+ /**
+ * #DB7093.
+ */
+ static const Color kPaleVioletRed;
+
+ /**
+ * #FFEFD5.
+ */
+ static const Color kPapayaWhip;
+
+ /**
+ * #FFDAB9.
+ */
+ static const Color kPeachPuff;
+
+ /**
+ * #CD853F.
+ */
+ static const Color kPeru;
+
+ /**
+ * #FFC0CB.
+ */
+ static const Color kPink;
+
+ /**
+ * #DDA0DD.
+ */
+ static const Color kPlum;
+
+ /**
+ * #B0E0E6.
+ */
+ static const Color kPowderBlue;
+
+ /**
+ * #800080.
+ */
+ static const Color kPurple;
+
+ /**
+ * #FF0000.
+ */
+ static const Color kRed;
+
+ /**
+ * #BC8F8F.
+ */
+ static const Color kRosyBrown;
+
+ /**
+ * #4169E1.
+ */
+ static const Color kRoyalBlue;
+
+ /**
+ * #8B4513.
+ */
+ static const Color kSaddleBrown;
+
+ /**
+ * #FA8072.
+ */
+ static const Color kSalmon;
+
+ /**
+ * #F4A460.
+ */
+ static const Color kSandyBrown;
+
+ /**
+ * #2E8B57.
+ */
+ static const Color kSeaGreen;
+
+ /**
+ * #FFF5EE.
+ */
+ static const Color kSeashell;
+
+ /**
+ * #A0522D.
+ */
+ static const Color kSienna;
+
+ /**
+ * #C0C0C0.
+ */
+ static const Color kSilver;
+
+ /**
+ * #87CEEB.
+ */
+ static const Color kSkyBlue;
+
+ /**
+ * #6A5ACD.
+ */
+ static const Color kSlateBlue;
+
+ /**
+ * #708090.
+ */
+ static const Color kSlateGray;
+
+ /**
+ * #FFFAFA.
+ */
+ static const Color kSnow;
+
+ /**
+ * #00FF7F.
+ */
+ static const Color kSpringGreen;
+
+ /**
+ * #4682B4.
+ */
+ static const Color kSteelBlue;
+
+ /**
+ * #D2B48C.
+ */
+ static const Color kTan;
+
+ /**
+ * #008080.
+ */
+ static const Color kTeal;
+
+ /**
+ * #D8BFD8.
+ */
+ static const Color kThistle;
+
+ /**
+ * #FF6347.
+ */
+ static const Color kTomato;
+
+ /**
+ * #40E0D0.
+ */
+ static const Color kTurquoise;
+
+ /**
+ * #EE82EE.
+ */
+ static const Color kViolet;
+
+ /**
+ * #F5DEB3.
+ */
+ static const Color kWheat;
+
+ /**
+ * #FFFFFF.
+ */
+ static const Color kWhite;
+
+ /**
+ * #F5F5F5.
+ */
+ static const Color kWhiteSmoke;
+
+ /**
+ * #FFFF00.
+ */
+ static const Color kYellow;
+
+ /**
+ * #9ACD32.
+ */
+ static const Color kYellowGreen;
+
+ constexpr Color() = default;
+
+ /**
+ * Constructs a Color.
+ *
+ * @param red Red value (0-1)
+ * @param green Green value (0-1)
+ * @param blue Blue value (0-1)
+ */
+ constexpr Color(double r, double g, double b)
+ : red(roundAndClamp(r)),
+ green(roundAndClamp(g)),
+ blue(roundAndClamp(b)) {}
+
+ double red = 0.0;
+ double green = 0.0;
+ double blue = 0.0;
+
+ private:
+ static constexpr double kPrecision = 1.0 / (1 << 12);
+
+ static constexpr double roundAndClamp(double value) {
+ const auto rounded =
+ (static_cast<int>(value / kPrecision) + 0.5) * kPrecision;
+ return std::clamp(rounded, 0.0, 1.0);
+ }
+};
+
+inline bool operator==(const Color& c1, const Color& c2) {
+ return c1.red == c2.red && c1.green == c2.green && c1.blue == c2.blue;
+}
+
+/*
+ * FIRST Colors
+ */
+inline constexpr Color Color::kDenim{0.0823529412, 0.376470589, 0.7411764706};
+inline constexpr Color Color::kFirstBlue{0.0, 0.4, 0.7019607844};
+inline constexpr Color Color::kFirstRed{0.9294117648, 0.1098039216,
+ 0.1411764706};
+
+/*
+ * Standard Colors
+ */
+inline constexpr Color Color::kAliceBlue{0.9411765f, 0.972549f, 1.0f};
+inline constexpr Color Color::kAntiqueWhite{0.98039216f, 0.92156863f,
+ 0.84313726f};
+inline constexpr Color Color::kAqua{0.0f, 1.0f, 1.0f};
+inline constexpr Color Color::kAquamarine{0.49803922f, 1.0f, 0.83137256f};
+inline constexpr Color Color::kAzure{0.9411765f, 1.0f, 1.0f};
+inline constexpr Color Color::kBeige{0.9607843f, 0.9607843f, 0.8627451f};
+inline constexpr Color Color::kBisque{1.0f, 0.89411765f, 0.76862746f};
+inline constexpr Color Color::kBlack{0.0f, 0.0f, 0.0f};
+inline constexpr Color Color::kBlanchedAlmond{1.0f, 0.92156863f, 0.8039216f};
+inline constexpr Color Color::kBlue{0.0f, 0.0f, 1.0f};
+inline constexpr Color Color::kBlueViolet{0.5411765f, 0.16862746f, 0.8862745f};
+inline constexpr Color Color::kBrown{0.64705884f, 0.16470589f, 0.16470589f};
+inline constexpr Color Color::kBurlywood{0.87058824f, 0.72156864f, 0.5294118f};
+inline constexpr Color Color::kCadetBlue{0.37254903f, 0.61960787f, 0.627451f};
+inline constexpr Color Color::kChartreuse{0.49803922f, 1.0f, 0.0f};
+inline constexpr Color Color::kChocolate{0.8235294f, 0.4117647f, 0.11764706f};
+inline constexpr Color Color::kCoral{1.0f, 0.49803922f, 0.3137255f};
+inline constexpr Color Color::kCornflowerBlue{0.39215687f, 0.58431375f,
+ 0.92941177f};
+inline constexpr Color Color::kCornsilk{1.0f, 0.972549f, 0.8627451f};
+inline constexpr Color Color::kCrimson{0.8627451f, 0.078431375f, 0.23529412f};
+inline constexpr Color Color::kCyan{0.0f, 1.0f, 1.0f};
+inline constexpr Color Color::kDarkBlue{0.0f, 0.0f, 0.54509807f};
+inline constexpr Color Color::kDarkCyan{0.0f, 0.54509807f, 0.54509807f};
+inline constexpr Color Color::kDarkGoldenrod{0.72156864f, 0.5254902f,
+ 0.043137256f};
+inline constexpr Color Color::kDarkGray{0.6627451f, 0.6627451f, 0.6627451f};
+inline constexpr Color Color::kDarkGreen{0.0f, 0.39215687f, 0.0f};
+inline constexpr Color Color::kDarkKhaki{0.7411765f, 0.7176471f, 0.41960785f};
+inline constexpr Color Color::kDarkMagenta{0.54509807f, 0.0f, 0.54509807f};
+inline constexpr Color Color::kDarkOliveGreen{0.33333334f, 0.41960785f,
+ 0.18431373f};
+inline constexpr Color Color::kDarkOrange{1.0f, 0.54901963f, 0.0f};
+inline constexpr Color Color::kDarkOrchid{0.6f, 0.19607843f, 0.8f};
+inline constexpr Color Color::kDarkRed{0.54509807f, 0.0f, 0.0f};
+inline constexpr Color Color::kDarkSalmon{0.9137255f, 0.5882353f, 0.47843137f};
+inline constexpr Color Color::kDarkSeaGreen{0.56078434f, 0.7372549f,
+ 0.56078434f};
+inline constexpr Color Color::kDarkSlateBlue{0.28235295f, 0.23921569f,
+ 0.54509807f};
+inline constexpr Color Color::kDarkSlateGray{0.18431373f, 0.30980393f,
+ 0.30980393f};
+inline constexpr Color Color::kDarkTurquoise{0.0f, 0.80784315f, 0.81960785f};
+inline constexpr Color Color::kDarkViolet{0.5803922f, 0.0f, 0.827451f};
+inline constexpr Color Color::kDeepPink{1.0f, 0.078431375f, 0.5764706f};
+inline constexpr Color Color::kDeepSkyBlue{0.0f, 0.7490196f, 1.0f};
+inline constexpr Color Color::kDimGray{0.4117647f, 0.4117647f, 0.4117647f};
+inline constexpr Color Color::kDodgerBlue{0.11764706f, 0.5647059f, 1.0f};
+inline constexpr Color Color::kFirebrick{0.69803923f, 0.13333334f, 0.13333334f};
+inline constexpr Color Color::kFloralWhite{1.0f, 0.98039216f, 0.9411765f};
+inline constexpr Color Color::kForestGreen{0.13333334f, 0.54509807f,
+ 0.13333334f};
+inline constexpr Color Color::kFuchsia{1.0f, 0.0f, 1.0f};
+inline constexpr Color Color::kGainsboro{0.8627451f, 0.8627451f, 0.8627451f};
+inline constexpr Color Color::kGhostWhite{0.972549f, 0.972549f, 1.0f};
+inline constexpr Color Color::kGold{1.0f, 0.84313726f, 0.0f};
+inline constexpr Color Color::kGoldenrod{0.85490197f, 0.64705884f, 0.1254902f};
+inline constexpr Color Color::kGray{0.5019608f, 0.5019608f, 0.5019608f};
+inline constexpr Color Color::kGreen{0.0f, 0.5019608f, 0.0f};
+inline constexpr Color Color::kGreenYellow{0.6784314f, 1.0f, 0.18431373f};
+inline constexpr Color Color::kHoneydew{0.9411765f, 1.0f, 0.9411765f};
+inline constexpr Color Color::kHotPink{1.0f, 0.4117647f, 0.7058824f};
+inline constexpr Color Color::kIndianRed{0.8039216f, 0.36078432f, 0.36078432f};
+inline constexpr Color Color::kIndigo{0.29411766f, 0.0f, 0.50980395f};
+inline constexpr Color Color::kIvory{1.0f, 1.0f, 0.9411765f};
+inline constexpr Color Color::kKhaki{0.9411765f, 0.9019608f, 0.54901963f};
+inline constexpr Color Color::kLavender{0.9019608f, 0.9019608f, 0.98039216f};
+inline constexpr Color Color::kLavenderBlush{1.0f, 0.9411765f, 0.9607843f};
+inline constexpr Color Color::kLawnGreen{0.4862745f, 0.9882353f, 0.0f};
+inline constexpr Color Color::kLemonChiffon{1.0f, 0.98039216f, 0.8039216f};
+inline constexpr Color Color::kLightBlue{0.6784314f, 0.84705883f, 0.9019608f};
+inline constexpr Color Color::kLightCoral{0.9411765f, 0.5019608f, 0.5019608f};
+inline constexpr Color Color::kLightCyan{0.8784314f, 1.0f, 1.0f};
+inline constexpr Color Color::kLightGoldenrodYellow{0.98039216f, 0.98039216f,
+ 0.8235294f};
+inline constexpr Color Color::kLightGray{0.827451f, 0.827451f, 0.827451f};
+inline constexpr Color Color::kLightGreen{0.5647059f, 0.93333334f, 0.5647059f};
+inline constexpr Color Color::kLightPink{1.0f, 0.7137255f, 0.75686276f};
+inline constexpr Color Color::kLightSalmon{1.0f, 0.627451f, 0.47843137f};
+inline constexpr Color Color::kLightSeagGeen{0.1254902f, 0.69803923f,
+ 0.6666667f};
+inline constexpr Color Color::kLightSkyBlue{0.5294118f, 0.80784315f,
+ 0.98039216f};
+inline constexpr Color Color::kLightSlateGray{0.46666667f, 0.53333336f, 0.6f};
+inline constexpr Color Color::kLightSteellue{0.6901961f, 0.76862746f,
+ 0.87058824f};
+inline constexpr Color Color::kLightYellow{1.0f, 1.0f, 0.8784314f};
+inline constexpr Color Color::kLime{0.0f, 1.0f, 0.0f};
+inline constexpr Color Color::kLimeGreen{0.19607843f, 0.8039216f, 0.19607843f};
+inline constexpr Color Color::kLinen{0.98039216f, 0.9411765f, 0.9019608f};
+inline constexpr Color Color::kMagenta{1.0f, 0.0f, 1.0f};
+inline constexpr Color Color::kMaroon{0.5019608f, 0.0f, 0.0f};
+inline constexpr Color Color::kMediumAquamarine{0.4f, 0.8039216f, 0.6666667f};
+inline constexpr Color Color::kMediumBlue{0.0f, 0.0f, 0.8039216f};
+inline constexpr Color Color::kMediumOrchid{0.7294118f, 0.33333334f, 0.827451f};
+inline constexpr Color Color::kMediumPurple{0.5764706f, 0.4392157f,
+ 0.85882354f};
+inline constexpr Color Color::kMediumSeaGreen{0.23529412f, 0.7019608f,
+ 0.44313726f};
+inline constexpr Color Color::kMediumSlateBlue{0.48235294f, 0.40784314f,
+ 0.93333334f};
+inline constexpr Color Color::kMediumSpringGreen{0.0f, 0.98039216f, 0.6039216f};
+inline constexpr Color Color::kMediumTurquoise{0.28235295f, 0.81960785f, 0.8f};
+inline constexpr Color Color::kMediumVioletRed{0.78039217f, 0.08235294f,
+ 0.52156866f};
+inline constexpr Color Color::kMidnightBlue{0.09803922f, 0.09803922f,
+ 0.4392157f};
+inline constexpr Color Color::kMintcream{0.9607843f, 1.0f, 0.98039216f};
+inline constexpr Color Color::kMistyRose{1.0f, 0.89411765f, 0.88235295f};
+inline constexpr Color Color::kMoccasin{1.0f, 0.89411765f, 0.70980394f};
+inline constexpr Color Color::kNavajoWhite{1.0f, 0.87058824f, 0.6784314f};
+inline constexpr Color Color::kNavy{0.0f, 0.0f, 0.5019608f};
+inline constexpr Color Color::kOldLace{0.99215686f, 0.9607843f, 0.9019608f};
+inline constexpr Color Color::kOlive{0.5019608f, 0.5019608f, 0.0f};
+inline constexpr Color Color::kOliveDrab{0.41960785f, 0.5568628f, 0.13725491f};
+inline constexpr Color Color::kOrange{1.0f, 0.64705884f, 0.0f};
+inline constexpr Color Color::kOrangeRed{1.0f, 0.27058825f, 0.0f};
+inline constexpr Color Color::kOrchid{0.85490197f, 0.4392157f, 0.8392157f};
+inline constexpr Color Color::kPaleGoldenrod{0.93333334f, 0.9098039f,
+ 0.6666667f};
+inline constexpr Color Color::kPaleGreen{0.59607846f, 0.9843137f, 0.59607846f};
+inline constexpr Color Color::kPaleTurquoise{0.6862745f, 0.93333334f,
+ 0.93333334f};
+inline constexpr Color Color::kPaleVioletRed{0.85882354f, 0.4392157f,
+ 0.5764706f};
+inline constexpr Color Color::kPapayaWhip{1.0f, 0.9372549f, 0.8352941f};
+inline constexpr Color Color::kPeachPuff{1.0f, 0.85490197f, 0.7254902f};
+inline constexpr Color Color::kPeru{0.8039216f, 0.52156866f, 0.24705882f};
+inline constexpr Color Color::kPink{1.0f, 0.7529412f, 0.79607844f};
+inline constexpr Color Color::kPlum{0.8666667f, 0.627451f, 0.8666667f};
+inline constexpr Color Color::kPowderBlue{0.6901961f, 0.8784314f, 0.9019608f};
+inline constexpr Color Color::kPurple{0.5019608f, 0.0f, 0.5019608f};
+inline constexpr Color Color::kRed{1.0f, 0.0f, 0.0f};
+inline constexpr Color Color::kRosyBrown{0.7372549f, 0.56078434f, 0.56078434f};
+inline constexpr Color Color::kRoyalBlue{0.25490198f, 0.4117647f, 0.88235295f};
+inline constexpr Color Color::kSaddleBrown{0.54509807f, 0.27058825f,
+ 0.07450981f};
+inline constexpr Color Color::kSalmon{0.98039216f, 0.5019608f, 0.44705883f};
+inline constexpr Color Color::kSandyBrown{0.95686275f, 0.6431373f, 0.3764706f};
+inline constexpr Color Color::kSeaGreen{0.18039216f, 0.54509807f, 0.34117648f};
+inline constexpr Color Color::kSeashell{1.0f, 0.9607843f, 0.93333334f};
+inline constexpr Color Color::kSienna{0.627451f, 0.32156864f, 0.1764706f};
+inline constexpr Color Color::kSilver{0.7529412f, 0.7529412f, 0.7529412f};
+inline constexpr Color Color::kSkyBlue{0.5294118f, 0.80784315f, 0.92156863f};
+inline constexpr Color Color::kSlateBlue{0.41568628f, 0.3529412f, 0.8039216f};
+inline constexpr Color Color::kSlateGray{0.4392157f, 0.5019608f, 0.5647059f};
+inline constexpr Color Color::kSnow{1.0f, 0.98039216f, 0.98039216f};
+inline constexpr Color Color::kSpringGreen{0.0f, 1.0f, 0.49803922f};
+inline constexpr Color Color::kSteelBlue{0.27450982f, 0.50980395f, 0.7058824f};
+inline constexpr Color Color::kTan{0.8235294f, 0.7058824f, 0.54901963f};
+inline constexpr Color Color::kTeal{0.0f, 0.5019608f, 0.5019608f};
+inline constexpr Color Color::kThistle{0.84705883f, 0.7490196f, 0.84705883f};
+inline constexpr Color Color::kTomato{1.0f, 0.3882353f, 0.2784314f};
+inline constexpr Color Color::kTurquoise{0.2509804f, 0.8784314f, 0.8156863f};
+inline constexpr Color Color::kViolet{0.93333334f, 0.50980395f, 0.93333334f};
+inline constexpr Color Color::kWheat{0.9607843f, 0.87058824f, 0.7019608f};
+inline constexpr Color Color::kWhite{1.0f, 1.0f, 1.0f};
+inline constexpr Color Color::kWhiteSmoke{0.9607843f, 0.9607843f, 0.9607843f};
+inline constexpr Color Color::kYellow{1.0f, 1.0f, 0.0f};
+inline constexpr Color Color::kYellowGreen{0.6039216f, 0.8039216f, 0.19607843f};
+
+} // namespace frc
diff --git a/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/util/Color8Bit.h b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/util/Color8Bit.h
new file mode 100644
index 0000000..d5dba07
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibc/src/main/native/include/frc/util/Color8Bit.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <algorithm>
+
+#include "Color.h"
+
+namespace frc {
+
+/**
+ * Represents colors that can be used with Addressable LEDs.
+ */
+class Color8Bit {
+ public:
+ constexpr Color8Bit() = default;
+
+ /**
+ * Constructs a Color8Bit.
+ *
+ * @param red Red value (0-255)
+ * @param green Green value (0-255)
+ * @param blue Blue value (0-255)
+ */
+ constexpr Color8Bit(int r, int g, int b)
+ : red(std::clamp(r, 0, 255)),
+ green(std::clamp(g, 0, 255)),
+ blue(std::clamp(b, 0, 255)) {}
+
+ /**
+ * Constructs a Color8Bit from a Color.
+ *
+ * @param color The color
+ */
+ constexpr Color8Bit(const Color& color)
+ : red(color.red * 255),
+ green(color.green * 255),
+ blue(color.blue * 255) {}
+
+ constexpr operator Color() const {
+ return Color(red / 255.0, green / 255.0, blue / 255.0);
+ }
+
+ int red = 0;
+ int green = 0;
+ int blue = 0;
+};
+
+inline bool operator==(const Color8Bit& c1, const Color8Bit& c2) {
+ return c1.red == c2.red && c1.green == c2.green && c1.blue == c2.blue;
+}
+
+} // namespace frc
diff --git a/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp b/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp
new file mode 100644
index 0000000..ae253a7
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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 <thread>
+
+#include "frc/SlewRateLimiter.h"
+#include "gtest/gtest.h"
+
+TEST(SlewRateLimiterTest, SlewRateLimitTest) {
+ frc::SlewRateLimiter<units::meters> limiter(1_mps);
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+ EXPECT_TRUE(limiter.Calculate(2_m) < 2_m);
+}
+
+TEST(SlewRateLimiterTest, SlewRateNoLimitTest) {
+ frc::SlewRateLimiter<units::meters> limiter(1_mps);
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+ EXPECT_EQ(limiter.Calculate(0.5_m), 0.5_m);
+}
diff --git a/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
index 319be79..f2f0a38 100644
--- a/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
+++ b/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -120,3 +120,14 @@
Pose2d end{4_m, 0_m, 0_rad};
Run(start, waypoints, end);
}
+
+TEST_F(CubicHermiteSplineTest, ThrowsOnMalformed) {
+ EXPECT_THROW(
+ Run(Pose2d{0_m, 0_m, Rotation2d(0_deg)}, std::vector<Translation2d>{},
+ Pose2d{1_m, 0_m, Rotation2d(180_deg)}),
+ SplineParameterizer::MalformedSplineException);
+ EXPECT_THROW(
+ Run(Pose2d{10_m, 10_m, Rotation2d(90_deg)}, std::vector<Translation2d>{},
+ Pose2d{10_m, 11_m, Rotation2d(-90_deg)}),
+ SplineParameterizer::MalformedSplineException);
+}
diff --git a/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
index cc10c6c..8a87053 100644
--- a/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
+++ b/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -85,3 +85,12 @@
Run(Pose2d(0_m, 0_m, Rotation2d(90_deg)),
Pose2d(-1_m, 0_m, Rotation2d(90_deg)));
}
+
+TEST_F(QuinticHermiteSplineTest, ThrowsOnMalformed) {
+ EXPECT_THROW(Run(Pose2d(0_m, 0_m, Rotation2d(0_deg)),
+ Pose2d(1_m, 0_m, Rotation2d(180_deg))),
+ SplineParameterizer::MalformedSplineException);
+ EXPECT_THROW(Run(Pose2d(10_m, 10_m, Rotation2d(90_deg)),
+ Pose2d(10_m, 11_m, Rotation2d(-90_deg))),
+ SplineParameterizer::MalformedSplineException);
+}
diff --git a/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
index 7e47e11..90046c5 100644
--- a/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
+++ b/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -33,3 +33,13 @@
12_fps_sq + 0.01_fps_sq);
}
}
+
+TEST(TrajectoryGenertionTest, ReturnsEmptyOnMalformed) {
+ const auto t = TrajectoryGenerator::GenerateTrajectory(
+ std::vector<Pose2d>{Pose2d(0_m, 0_m, Rotation2d(0_deg)),
+ Pose2d(1_m, 0_m, Rotation2d(180_deg))},
+ TrajectoryConfig(12_fps, 12_fps_sq));
+
+ ASSERT_EQ(t.States().size(), 1u);
+ ASSERT_EQ(t.TotalTime(), 0_s);
+}
diff --git a/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp b/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
new file mode 100644
index 0000000..af69b58
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include <vector>
+
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/TrajectoryConfig.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+void TestSameShapedTrajectory(std::vector<frc::Trajectory::State> statesA,
+ std::vector<frc::Trajectory::State> statesB) {
+ for (unsigned int i = 0; i < statesA.size() - 1; i++) {
+ auto a1 = statesA[i].pose;
+ auto a2 = statesA[i + 1].pose;
+
+ auto b1 = statesB[i].pose;
+ auto b2 = statesB[i + 1].pose;
+
+ auto a = a2.RelativeTo(a1);
+ auto b = b2.RelativeTo(b1);
+
+ EXPECT_NEAR(a.Translation().X().to<double>(),
+ b.Translation().X().to<double>(), 1E-9);
+ EXPECT_NEAR(a.Translation().Y().to<double>(),
+ b.Translation().Y().to<double>(), 1E-9);
+ EXPECT_NEAR(a.Rotation().Radians().to<double>(),
+ b.Rotation().Radians().to<double>(), 1E-9);
+ }
+}
+
+TEST(TrajectoryTransforms, TransformBy) {
+ frc::TrajectoryConfig config{3_mps, 3_mps_sq};
+ auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ frc::Pose2d{}, {}, frc::Pose2d{1_m, 1_m, frc::Rotation2d(90_deg)},
+ config);
+
+ auto transformedTrajectory =
+ trajectory.TransformBy({{1_m, 2_m}, frc::Rotation2d(30_deg)});
+
+ auto firstPose = transformedTrajectory.Sample(0_s).pose;
+
+ EXPECT_NEAR(firstPose.Translation().X().to<double>(), 1.0, 1E-9);
+ EXPECT_NEAR(firstPose.Translation().Y().to<double>(), 2.0, 1E-9);
+ EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 30.0, 1E-9);
+
+ TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
+}
+
+TEST(TrajectoryTransforms, RelativeTo) {
+ frc::TrajectoryConfig config{3_mps, 3_mps_sq};
+ auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ frc::Pose2d{1_m, 2_m, frc::Rotation2d(30_deg)}, {},
+ frc::Pose2d{5_m, 7_m, frc::Rotation2d(90_deg)}, config);
+
+ auto transformedTrajectory =
+ trajectory.RelativeTo({1_m, 2_m, frc::Rotation2d(30_deg)});
+
+ auto firstPose = transformedTrajectory.Sample(0_s).pose;
+
+ EXPECT_NEAR(firstPose.Translation().X().to<double>(), 0, 1E-9);
+ EXPECT_NEAR(firstPose.Translation().Y().to<double>(), 0, 1E-9);
+ EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 0, 1E-9);
+
+ TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
+}
diff --git a/third_party/allwpilib_2019/wpilibcExamples/build.gradle b/third_party/allwpilib_2019/wpilibcExamples/build.gradle
index 12279df..c6c1dff 100644
--- a/third_party/allwpilib_2019/wpilibcExamples/build.gradle
+++ b/third_party/allwpilib_2019/wpilibcExamples/build.gradle
@@ -6,6 +6,8 @@
apply plugin: 'edu.wpi.first.NativeUtils'
apply plugin: ExtraTasks
+evaluationDependsOn(':hal')
+
apply from: '../shared/config.gradle'
ext.examplesMap = [:]
@@ -222,6 +224,25 @@
}
apply from: 'publish.gradle'
+model {
+ tasks {
+ def c = $.components
+ project.tasks.register('buildDesktopCpp') { compileTask->
+ def systemArch = getCurrentArch()
+ c.each {
+ if (it in NativeExecutableSpec && it.name) {
+ it.binaries.each {
+ def arch = it.targetPlatform.name
+ if (arch == systemArch && it.buildType.name == 'debug') {
+ compileTask.dependsOn it.tasks.link
+ }
+ }
+ }
+ }
+ }
+ }
+}
+
ext {
templateDirectory = new File("$projectDir/src/main/cpp/templates/")
templateFile = new File("$projectDir/src/main/cpp/templates/templates.json")
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/command2/ReplaceMeCommand2.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/command2/ReplaceMeCommand2.cpp
new file mode 100644
index 0000000..e8085a1
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/command2/ReplaceMeCommand2.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "ReplaceMeCommand2.h"
+
+ReplaceMeCommand2::ReplaceMeCommand2() {
+ // Use addRequirements() here to declare subsystem dependencies.
+}
+
+// Called when the command is initially scheduled.
+void ReplaceMeCommand2::Initialize() {}
+
+// Called repeatedly when this Command is scheduled to run
+void ReplaceMeCommand2::Execute() {}
+
+// Called once the command ends or is interrupted.
+void ReplaceMeCommand2::End(bool interrupted) {}
+
+// Returns true when the command should end.
+bool ReplaceMeCommand2::IsFinished() { return false; }
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/command2/ReplaceMeCommand2.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/command2/ReplaceMeCommand2.h
new file mode 100644
index 0000000..fd041fa
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/command2/ReplaceMeCommand2.h
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+/**
+ * An example command.
+ *
+ * <p>Note that this extends CommandHelper, rather extending CommandBase
+ * directly; this is crucially important, or else the decorator functions in
+ * Command will *not* work!
+ */
+class ReplaceMeCommand2
+ : public frc2::CommandHelper<frc2::CommandBase, ReplaceMeCommand2> {
+ public:
+ ReplaceMeCommand2();
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+};
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/commands.json b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/commands.json
index bb8cbb6..d039a73 100644
--- a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/commands.json
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/commands.json
@@ -12,10 +12,11 @@
"source": [
"ReplaceMeEmptyClass.cpp"
],
- "replacename": "ReplaceMeEmptyClass"
+ "replacename": "ReplaceMeEmptyClass",
+ "commandversion": 0
},
{
- "name": "Command",
+ "name": "Command (Old)",
"description": "Create a base command",
"tags": [
"command"
@@ -27,10 +28,11 @@
"source": [
"ReplaceMeCommand.cpp"
],
- "replacename": "ReplaceMeCommand"
+ "replacename": "ReplaceMeCommand",
+ "commandversion": 1
},
{
- "name": "Command Group",
+ "name": "Command Group (Old)",
"description": "Create a command group",
"tags": [
"commandgroup"
@@ -42,10 +44,11 @@
"source": [
"ReplaceMeCommandGroup.cpp"
],
- "replacename": "ReplaceMeCommandGroup"
+ "replacename": "ReplaceMeCommandGroup",
+ "commandversion": 1
},
{
- "name": "Instant Command",
+ "name": "Instant Command (Old)",
"description": "A command that runs immediately",
"tags": [
"instantcommand"
@@ -57,10 +60,11 @@
"source": [
"ReplaceMeInstantCommand.cpp"
],
- "replacename": "ReplaceMeInstantCommand"
+ "replacename": "ReplaceMeInstantCommand",
+ "commandversion": 1
},
{
- "name": "Subsystem",
+ "name": "Subsystem (Old)",
"description": "A subsystem",
"tags": [
"subsystem"
@@ -72,10 +76,11 @@
"source": [
"ReplaceMeSubsystem.cpp"
],
- "replacename": "ReplaceMeSubsystem"
+ "replacename": "ReplaceMeSubsystem",
+ "commandversion": 1
},
{
- "name": "PID Subsystem",
+ "name": "PID Subsystem (Old)",
"description": "A subsystem that runs a PID loop",
"tags": [
"pidsubsystem",
@@ -88,10 +93,11 @@
"source": [
"ReplaceMePIDSubsystem.cpp"
],
- "replacename": "ReplaceMePIDSubsystem"
+ "replacename": "ReplaceMePIDSubsystem",
+ "commandversion": 1
},
{
- "name": "Timed Command",
+ "name": "Timed Command (Old)",
"description": "A command that runs for a specified time",
"tags": [
"timedcommand"
@@ -103,10 +109,11 @@
"source": [
"ReplaceMeTimedCommand.cpp"
],
- "replacename": "ReplaceMeTimedCommand"
+ "replacename": "ReplaceMeTimedCommand",
+ "commandversion": 1
},
{
- "name": "Trigger",
+ "name": "Trigger (Old)",
"description": "A command that runs off of a trigger",
"tags": [
"trigger"
@@ -118,6 +125,215 @@
"source": [
"ReplaceMeTrigger.cpp"
],
- "replacename": "ReplaceMeTrigger"
+ "replacename": "ReplaceMeTrigger",
+ "commandversion": 1
+ },
+ {
+ "name": "Command (New)",
+ "description": "A command.",
+ "tags": [
+ "command"
+ ],
+ "foldername": "command2",
+ "headers": [
+ "ReplaceMeCommand2.h"
+ ],
+ "source": [
+ "ReplaceMeCommand2.cpp"
+ ],
+ "replacename": "ReplaceMeCommand2",
+ "commandversion": 2
+ },
+ {
+ "name": "InstantCommand (New)",
+ "description": "A command that finishes instantly.",
+ "tags": [
+ "instantcommand"
+ ],
+ "foldername": "instantcommand",
+ "headers": [
+ "ReplaceMeInstantCommand2.h"
+ ],
+ "source": [
+ "ReplaceMeInstantCommand2.cpp"
+ ],
+ "replacename": "ReplaceMeInstantCommand2",
+ "commandversion": 2
+ },
+ {
+ "name": "ParallelCommandGroup (New)",
+ "description": "A command group that runs commands in parallel, ending when all commands have finished.",
+ "tags": [
+ "parallelcommandgroup"
+ ],
+ "foldername": "parallelcommandgroup",
+ "headers": [
+ "ReplaceMeParallelCommandGroup.h"
+ ],
+ "source": [
+ "ReplaceMeParallelCommandGroup.cpp"
+ ],
+ "replacename": "ReplaceMeParallelCommandGroup",
+ "commandversion": 2
+ },
+ {
+ "name": "ParallelDeadlineGroup (New)",
+ "description": "A command group that runs commands in parallel, ending when a specific command has finished.",
+ "tags": [
+ "paralleldeadlinegroup"
+ ],
+ "foldername": "paralleldeadlinegroup",
+ "headers": [
+ "ReplaceMeParallelDeadlineGroup.h"
+ ],
+ "source": [
+ "ReplaceMeParallelDeadlineGroup.cpp"
+ ],
+ "replacename": "ReplaceMeParallelDeadlineGroup",
+ "commandversion": 2
+ },
+ {
+ "name": "ParallelRaceGroup (New)",
+ "description": "A command that runs commands in parallel, ending as soon as any command has finished.",
+ "tags": [
+ "parallelracegroup"
+ ],
+ "foldername": "parallelracegroup",
+ "headers": [
+ "ReplaceMeParallelRaceGroup.h"
+ ],
+ "source": [
+ "ReplaceMeParallelRaceGroup.cpp"
+ ],
+ "replacename": "ReplaceMeParallelRaceGroup",
+ "commandversion": 2
+ },
+ {
+ "name": "PIDCommand (New)",
+ "description": "A command that runs a PIDController.",
+ "tags": [
+ "pidcommand"
+ ],
+ "foldername": "pidcommand",
+ "headers": [
+ "ReplaceMePIDCommand.h"
+ ],
+ "source": [
+ "ReplaceMePIDCommand.cpp"
+ ],
+ "replacename": "ReplaceMePIDCommand",
+ "commandversion": 2
+ },
+ {
+ "name": "PIDSubsystem (New)",
+ "description": "A subsystem that runs a PIDController.",
+ "tags": [
+ "pidsubsystem"
+ ],
+ "foldername": "pidsubsystem2",
+ "headers": [
+ "ReplaceMePIDSubsystem2.h"
+ ],
+ "source": [
+ "ReplaceMePIDSubsystem2.cpp"
+ ],
+ "replacename": "ReplaceMePIDSubsystem2",
+ "commandversion": 2
+ },
+ {
+ "name": "ProfiledPIDCommand (New)",
+ "description": "A command that runs a ProfiledPIDController.",
+ "tags": [
+ "profiledpidcommand"
+ ],
+ "foldername": "profiledpidcommand",
+ "headers": [
+ "ReplaceMeProfiledPIDCommand.h"
+ ],
+ "source": [
+ "ReplaceMeProfiledPIDCommand.cpp"
+ ],
+ "replacename": "ReplaceMeProfiledPIDCommand",
+ "commandversion": 2
+ },
+ {
+ "name": "ProfiledPIDSubsystem (New)",
+ "description": "A subsystem that runs a ProfiledPIDController.",
+ "tags": [
+ "profiledpidsubsystem"
+ ],
+ "foldername": "profiledpidsubsystem",
+ "headers": [
+ "ReplaceMeProfiledPIDSubsystem.h"
+ ],
+ "source": [
+ "ReplaceMeProfiledPIDSubsystem.cpp"
+ ],
+ "replacename": "ReplaceMeProfiledPIDSubsystem",
+ "commandversion": 2
+ },
+ {
+ "name": "SequentialCommandGroup (New)",
+ "description": "A command group that runs commands in sequence.",
+ "tags": [
+ "sequentialcommandgroup"
+ ],
+ "foldername": "sequentialcommandgroup",
+ "headers": [
+ "ReplaceMeSequentialCommandGroup.h"
+ ],
+ "source": [
+ "ReplaceMeSequentialCommandGroup.cpp"
+ ],
+ "replacename": "ReplaceMeSequentialCommandGroup",
+ "commandversion": 2
+ },
+ {
+ "name": "Subsystem (New)",
+ "description": "A robot subsystem.",
+ "tags": [
+ "subsystem"
+ ],
+ "foldername": "subsystem2",
+ "headers": [
+ "ReplaceMeSubsystem2.h"
+ ],
+ "source": [
+ "ReplaceMeSubsystem2.cpp"
+ ],
+ "replacename": "ReplaceMeSubsystem2",
+ "commandversion": 2
+ },
+ {
+ "name": "TrapezoidProfileCommand (New)",
+ "description": "A command that executes a trapezoidal motion profile.",
+ "tags": [
+ "trapezoidprofilecommand"
+ ],
+ "foldername": "trapezoidprofilecommand",
+ "headers": [
+ "ReplaceMeTrapezoidProfileCommand.h"
+ ],
+ "source": [
+ "ReplaceMeTrapezoidProfileCommand.cpp"
+ ],
+ "replacename": "ReplaceMeTrapezoidProfileCommand",
+ "commandversion": 2
+ },
+ {
+ "name": "TrapezoidProfileSubsystem (New)",
+ "description": "A command that executes a trapezoidal motion profile.",
+ "tags": [
+ "trapezoidprofilesubsystem"
+ ],
+ "foldername": "trapezoidprofilesubsystem",
+ "headers": [
+ "ReplaceMeTrapezoidProfileSubsystem.h"
+ ],
+ "source": [
+ "ReplaceMeTrapezoidProfileSubsystem.cpp"
+ ],
+ "replacename": "ReplaceMeTrapezoidProfileSubsystem",
+ "commandversion": 2
}
]
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.cpp
new file mode 100644
index 0000000..bf54a0a
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.cpp
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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 "ReplaceMeInstantCommand2.h"
+
+// NOTE: Consider using this command inline, rather than writing a subclass.
+// For more information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+ReplaceMeInstantCommand2::ReplaceMeInstantCommand2() {
+ // Use addRequirements() here to declare subsystem dependencies.
+}
+
+// Called when the command is initially scheduled.
+void ReplaceMeInstantCommand2::Initialize() {}
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.h
new file mode 100644
index 0000000..dec11c2
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.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/InstantCommand.h>
+
+class ReplaceMeInstantCommand2
+ : public frc2::CommandHelper<frc2::InstantCommand,
+ ReplaceMeInstantCommand2> {
+ public:
+ ReplaceMeInstantCommand2();
+
+ void Initialize() override;
+};
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.cpp
new file mode 100644
index 0000000..aa99a3c
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.cpp
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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 "ReplaceMeParallelCommandGroup.h"
+
+// NOTE: Consider using this command inline, rather than writing a subclass.
+// For more information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+ReplaceMeParallelCommandGroup::ReplaceMeParallelCommandGroup() {
+ // Add your commands here, e.g.
+ // AddCommands(FooCommand(), BarCommand());
+}
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.h
new file mode 100644
index 0000000..a4f9519
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.h
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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/ParallelCommandGroup.h>
+
+class ReplaceMeParallelCommandGroup
+ : public frc2::CommandHelper<frc2::ParallelCommandGroup,
+ ReplaceMeParallelCommandGroup> {
+ public:
+ ReplaceMeParallelCommandGroup();
+};
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.cpp
new file mode 100644
index 0000000..1cfcfa0
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "ReplaceMeParallelDeadlineGroup.h"
+
+#include <frc2/command/InstantCommand.h>
+
+// NOTE: Consider using this command inline, rather than writing a subclass.
+// For more information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+ReplaceMeParallelDeadlineGroup::ReplaceMeParallelDeadlineGroup()
+ : CommandHelper(
+ // The deadline command
+ frc2::InstantCommand([] {})) {
+ // Add your commands here, e.g.
+ // AddCommands(FooCommand(), BarCommand());
+}
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.h
new file mode 100644
index 0000000..7ecebb4
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.h
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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/ParallelDeadlineGroup.h>
+
+class ReplaceMeParallelDeadlineGroup
+ : public frc2::CommandHelper<frc2::ParallelDeadlineGroup,
+ ReplaceMeParallelDeadlineGroup> {
+ public:
+ ReplaceMeParallelDeadlineGroup();
+};
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.cpp
new file mode 100644
index 0000000..f3f1d86
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.cpp
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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 "ReplaceMeParallelRaceGroup.h"
+
+// NOTE: Consider using this command inline, rather than writing a subclass.
+// For more information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+ReplaceMeParallelRaceGroup::ReplaceMeParallelRaceGroup() {
+ // Add your commands here, e.g.
+ // AddCommands(FooCommand(), BarCommand());
+}
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.h
new file mode 100644
index 0000000..0b13b18
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.h
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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/ParallelRaceGroup.h>
+
+class ReplaceMeParallelRaceGroup
+ : public frc2::CommandHelper<frc2::ParallelRaceGroup,
+ ReplaceMeParallelRaceGroup> {
+ public:
+ ReplaceMeParallelRaceGroup();
+};
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.cpp
new file mode 100644
index 0000000..b465b1e
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "ReplaceMePIDCommand.h"
+
+// NOTE: Consider using this command inline, rather than writing a subclass.
+// For more information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+ReplaceMePIDCommand::ReplaceMePIDCommand()
+ : CommandHelper(frc2::PIDController(0, 0, 0),
+ // This should return the measurement
+ [] { return 0; },
+ // This should return the setpoint (can also be a constant)
+ [] { return 0; },
+ // This uses the output
+ [](double output) {
+ // Use the output here
+ }) {}
+
+// Returns true when the command should end.
+bool ReplaceMePIDCommand::IsFinished() { return false; }
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.h
new file mode 100644
index 0000000..03c874c
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.h
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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/PIDCommand.h>
+
+class ReplaceMePIDCommand
+ : public frc2::CommandHelper<frc2::PIDCommand, ReplaceMePIDCommand> {
+ public:
+ ReplaceMePIDCommand();
+
+ bool IsFinished() override;
+};
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/pidsubsystem2/ReplaceMePIDSubsystem2.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/pidsubsystem2/ReplaceMePIDSubsystem2.cpp
new file mode 100644
index 0000000..0fa81a2
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/pidsubsystem2/ReplaceMePIDSubsystem2.cpp
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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 "ReplaceMePIDSubsystem2.h"
+
+ReplaceMePIDSubsystem2::ReplaceMePIDSubsystem2()
+ : PIDSubsystem(
+ // The PIDController used by the subsystem
+ frc2::PIDController(0, 0, 0)) {}
+
+void ReplaceMePIDSubsystem2::UseOutput(double output, double setpoint) {
+ // Use the output here
+}
+
+double ReplaceMePIDSubsystem2::GetMeasurement() {
+ // Return the process variable measurement here
+ return 0;
+}
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/pidsubsystem2/ReplaceMePIDSubsystem2.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/pidsubsystem2/ReplaceMePIDSubsystem2.h
new file mode 100644
index 0000000..6c8e344
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/pidsubsystem2/ReplaceMePIDSubsystem2.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/PIDSubsystem.h>
+
+class ReplaceMePIDSubsystem2 : public frc2::PIDSubsystem {
+ public:
+ ReplaceMePIDSubsystem2();
+
+ protected:
+ void UseOutput(double output, double setpoint) override;
+
+ double GetMeasurement() override;
+};
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.cpp
new file mode 100644
index 0000000..392e643
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.cpp
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "ReplaceMeProfiledPIDCommand.h"
+
+// NOTE: Consider using this command inline, rather than writing a subclass.
+// For more information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+ReplaceMeProfiledPIDCommand::ReplaceMeProfiledPIDCommand()
+ : CommandHelper(
+ // The ProfiledPIDController that the command will use
+ frc::ProfiledPIDController<units::meter>(
+ // The PID gains
+ 0, 0, 0,
+ // The motion profile constraints
+ {0_mps, 0_mps_sq}),
+ // This should return the measurement
+ [] { return 0_m; },
+ // This should return the goal state (can also be a constant)
+ [] {
+ return frc::TrapezoidProfile<units::meter>::State{0_m, 0_mps};
+ },
+ // This uses the output and current trajectory setpoint
+ [](double output,
+ frc::TrapezoidProfile<units::meter>::State setpoint) {
+ // Use the output and setpoint here
+ }) {}
+
+// Returns true when the command should end.
+bool ReplaceMeProfiledPIDCommand::IsFinished() { return false; }
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.h
new file mode 100644
index 0000000..d3407e8
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.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/ProfiledPIDCommand.h>
+
+class ReplaceMeProfiledPIDCommand
+ : public frc2::CommandHelper<frc2::ProfiledPIDCommand<units::meter>,
+ ReplaceMeProfiledPIDCommand> {
+ public:
+ ReplaceMeProfiledPIDCommand();
+
+ bool IsFinished() override;
+};
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.cpp
new file mode 100644
index 0000000..dc27b43
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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 "ReplaceMeProfiledPIDSubsystem.h"
+
+ReplaceMeProfiledPIDSubsystem::ReplaceMeProfiledPIDSubsystem()
+ : ProfiledPIDSubsystem(
+ // The ProfiledPIDController used by the subsystem
+ frc::ProfiledPIDController<units::meter>(
+ // The PID gains
+ 0, 0, 0,
+ // The constraints for the motion profiles
+ {0_mps, 0_mps_sq})) {}
+
+void ReplaceMeProfiledPIDSubsystem::UseOutput(
+ double output, frc::TrapezoidProfile<units::meter>::State setpoint) {
+ // Use the output and current trajectory setpoint here
+}
+
+units::meter_t ReplaceMeProfiledPIDSubsystem::GetMeasurement() {
+ // Return the process variable measurement here
+ return 0_m;
+}
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.h
new file mode 100644
index 0000000..b382b1d
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.h
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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/ProfiledPIDSubsystem.h>
+
+class ReplaceMeProfiledPIDSubsystem
+ : public frc2::ProfiledPIDSubsystem<units::meter> {
+ public:
+ ReplaceMeProfiledPIDSubsystem();
+
+ protected:
+ void UseOutput(double output,
+ frc::TrapezoidProfile<units::meter>::State setpoint) override;
+
+ units::meter_t GetMeasurement() override;
+};
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.cpp
new file mode 100644
index 0000000..3a5fadc
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.cpp
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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 "ReplaceMeSequentialCommandGroup.h"
+
+// NOTE: Consider using this command inline, rather than writing a subclass.
+// For more information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+ReplaceMeSequentialCommandGroup::ReplaceMeSequentialCommandGroup() {
+ // Add your commands here, e.g.
+ // AddCommands(FooCommand(), BarCommand());
+}
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.h
new file mode 100644
index 0000000..5e629b8
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.h
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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/SequentialCommandGroup.h>
+
+class ReplaceMeSequentialCommandGroup
+ : public frc2::CommandHelper<frc2::SequentialCommandGroup,
+ ReplaceMeSequentialCommandGroup> {
+ public:
+ ReplaceMeSequentialCommandGroup();
+};
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/subsystem2/ReplaceMeSubsystem2.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/subsystem2/ReplaceMeSubsystem2.cpp
new file mode 100644
index 0000000..5d6805a
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/subsystem2/ReplaceMeSubsystem2.cpp
@@ -0,0 +1,13 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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 "ReplaceMeSubsystem2.h"
+
+ReplaceMeSubsystem2::ReplaceMeSubsystem2() {}
+
+// This method will be called once per scheduler run
+void ReplaceMeSubsystem2::Periodic() {}
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/subsystem2/ReplaceMeSubsystem2.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/subsystem2/ReplaceMeSubsystem2.h
new file mode 100644
index 0000000..6f15f46
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/subsystem2/ReplaceMeSubsystem2.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 <frc2/command/SubsystemBase.h>
+
+class ReplaceMeSubsystem2 : public frc2::SubsystemBase {
+ public:
+ ReplaceMeSubsystem2();
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic();
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+};
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp
new file mode 100644
index 0000000..b8cb678
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "ReplaceMeTrapezoidProfileCommand.h"
+
+// NOTE: Consider using this command inline, rather than writing a subclass.
+// For more information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+ReplaceMeTrapezoidProfileCommand::ReplaceMeTrapezoidProfileCommand()
+ : CommandHelper
+ // The profile to execute
+ (frc::TrapezoidProfile<units::meter>(
+ // The maximum velocity and acceleration of the profile
+ {5_mps, 5_mps_sq},
+ // The goal state of the profile
+ {10_m, 0_mps},
+ // The initial state of the profile
+ {0_m, 0_mps}),
+ [](frc::TrapezoidProfile<units::meter>::State state) {
+ // Use the computed intermediate trajectory state here
+ }) {}
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.h
new file mode 100644
index 0000000..e9043be
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.h
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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>
+
+class ReplaceMeTrapezoidProfileCommand
+ : public frc2::CommandHelper<frc2::TrapezoidProfileCommand<units::meter>,
+ ReplaceMeTrapezoidProfileCommand> {
+ public:
+ ReplaceMeTrapezoidProfileCommand();
+};
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.cpp
new file mode 100644
index 0000000..162c02f
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.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 "ReplaceMeTrapezoidProfileSubsystem.h"
+
+ReplaceMeTrapezoidProfileSubsystem::ReplaceMeTrapezoidProfileSubsystem()
+ : TrapezoidProfileSubsystem(
+ // The motion profile constraints
+ {5_mps, 5_mps_sq},
+ // The initial position of the mechanism
+ 0_m) {}
+
+void ReplaceMeTrapezoidProfileSubsystem::UseState(
+ frc::TrapezoidProfile<units::meters>::State state) {
+ // Use the current profile state here
+}
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.h
new file mode 100644
index 0000000..e93f590
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.h
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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/TrapezoidProfileSubsystem.h>
+
+class ReplaceMeTrapezoidProfileSubsystem
+ : frc2::TrapezoidProfileSubsystem<units::meters> {
+ public:
+ ReplaceMeTrapezoidProfileSubsystem();
+
+ protected:
+ void UseState(frc::TrapezoidProfile<units::meters>::State state) override;
+};
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp
index 3cf84e7..3658824 100644
--- a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp
@@ -14,7 +14,7 @@
class Robot : public frc::TimedRobot {
static constexpr int kLength = 60;
- // PWM port 0
+ // PWM port 9
// Must be a PWM header, not MXP or DIO
frc::AddressableLED m_led{9};
std::array<frc::AddressableLED::LEDData, kLength>
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Constants.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Constants.cpp
new file mode 100644
index 0000000..0cf9b96
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Constants.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#include "Constants.h"
+
+namespace DriveConstants {
+
+const frc::MecanumDriveKinematics 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)};
+
+} // namespace DriveConstants
+
+namespace AutoConstants {
+
+const frc::TrapezoidProfile<units::radians>::Constraints
+ kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration};
+
+} // namespace AutoConstants
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
index c7379cb..756deaa 100644
--- a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
@@ -22,12 +22,6 @@
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
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
index 5f5f05f..1baa3de 100644
--- a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
@@ -83,8 +83,8 @@
constexpr double kPYController = 0.5;
constexpr double kPThetaController = 0.5;
-constexpr frc::TrapezoidProfile<units::radians>::Constraints
- kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration};
+extern const frc::TrapezoidProfile<units::radians>::Constraints
+ kThetaControllerConstraints;
} // namespace AutoConstants
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Constants.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Constants.cpp
new file mode 100644
index 0000000..46ffde3
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Constants.cpp
@@ -0,0 +1,13 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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 "Constants.h"
+
+using namespace DriveConstants;
+
+const frc::DifferentialDriveKinematics DriveConstants::kDriveKinematics(
+ kTrackwidth);
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
index 71da410..33d53c6 100644
--- a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
@@ -18,6 +18,8 @@
#include <frc2/command/SequentialCommandGroup.h>
#include <frc2/command/button/JoystickButton.h>
+#include "Constants.h"
+
RobotContainer::RobotContainer() {
// Initialize all of your commands and subsystems here
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
index 22564c6..649082b 100644
--- a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
@@ -32,8 +32,8 @@
constexpr bool kLeftEncoderReversed = false;
constexpr bool kRightEncoderReversed = true;
-constexpr auto kTrackwidth = 0.6_m;
-constexpr frc::DifferentialDriveKinematics kDriveKinematics(kTrackwidth);
+constexpr auto kTrackwidth = 0.69_m;
+extern const frc::DifferentialDriveKinematics kDriveKinematics;
constexpr int kEncoderCPR = 1024;
constexpr double kWheelDiameterInches = 6;
@@ -45,15 +45,15 @@
// 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
+// theoretically for *your* robot's drive. The Robot 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;
+constexpr auto ks = 0.22_V;
+constexpr auto kv = 1.98 * 1_V * 1_s / 1_m;
+constexpr auto ka = 0.2 * 1_V * 1_s * 1_s / 1_m;
// Example value only - as above, this must be tuned for your drive!
-constexpr double kPDriveVel = 0.5;
+constexpr double kPDriveVel = 8.5;
} // namespace DriveConstants
namespace AutoConstants {
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Constants.cpp b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Constants.cpp
new file mode 100644
index 0000000..a60d803
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Constants.cpp
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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 "Constants.h"
+
+namespace AutoConstants {
+
+const frc::TrapezoidProfile<units::radians>::Constraints
+ kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration};
+
+} // namespace AutoConstants
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
index ed1f880..51b7030 100644
--- a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
@@ -103,8 +103,8 @@
//
-constexpr frc::TrapezoidProfile<units::radians>::Constraints
- kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration};
+extern const frc::TrapezoidProfile<units::radians>::Constraints
+ kThetaControllerConstraints;
} // namespace AutoConstants
diff --git a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h
index 763eafd..f6cd550 100644
--- a/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h
+++ b/third_party/allwpilib_2019/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h
@@ -18,8 +18,6 @@
*/
void Periodic() override;
- // Subsystem methods go here.
-
private:
// Components (e.g. motor controllers and sensors) should generally be
// declared private and exposed only through public methods.
diff --git a/third_party/allwpilib_2019/wpilibj/build.gradle b/third_party/allwpilib_2019/wpilibj/build.gradle
index cf6c4e5..b2e22fb 100644
--- a/third_party/allwpilib_2019/wpilibj/build.gradle
+++ b/third_party/allwpilib_2019/wpilibj/build.gradle
@@ -170,3 +170,5 @@
showStandardStreams = true
}
}
+
+apply from: "${rootDir}/shared/javaDesktopTestTask.gradle"
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java
index 8f9c1b4..3cc0763 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java
@@ -7,6 +7,9 @@
package edu.wpi.first.wpilibj;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
/**
* Buffer storage for Addressable LEDs.
*/
@@ -26,9 +29,9 @@
* 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]
+ * @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) {
@@ -42,9 +45,9 @@
* 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]
+ * @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) {
@@ -83,6 +86,29 @@
}
/**
+ * Sets a specific LED in the buffer.
+ *
+ * @param index The index to write
+ * @param color The color of the LED
+ */
+ public void setLED(int index, Color color) {
+ setRGB(index,
+ (int) (color.red * 255),
+ (int) (color.green * 255),
+ (int) (color.blue * 255));
+ }
+
+ /**
+ * Sets a specific LED in the buffer.
+ *
+ * @param index The index to write
+ * @param color The color of the LED
+ */
+ public void setLED(int index, Color8Bit color) {
+ setRGB(index, color.red, color.green, color.blue);
+ }
+
+ /**
* Gets the buffer length.
*
* @return the buffer length
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonFX.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonFX.java
new file mode 100644
index 0000000..03acab2
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonFX.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Cross the Road Electronics (CTRE) Talon FX Speed Controller with PWM control.
+ *
+ * <p>Note that the TalonFX uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the TalonFX User Manual
+ * available from CTRE.
+ *
+ * <p><ul>
+ * <li>2.004ms = full "forward"
+ * <li>1.520ms = the "high end" of the deadband range
+ * <li>1.500ms = center of the deadband range (off)
+ * <li>1.480ms = the "low end" of the deadband range
+ * <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class PWMTalonFX extends PWMSpeedController {
+ /**
+ * Constructor for a TalonFX connected via PWM.
+ *
+ * @param channel The PWM channel that the Talon FX is attached to. 0-9 are on-board, 10-19 are
+ * on the MXP port
+ */
+ public PWMTalonFX(final int channel) {
+ super(channel);
+
+ setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_TalonFX, getChannel() + 1);
+ SendableRegistry.setName(this, "PWMTalonFX", getChannel());
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVenom.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVenom.java
new file mode 100644
index 0000000..9a1116d
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVenom.java
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Playing with Fusion Venom Smart Motor with PWM control.
+ *
+ * <p>Note that the Venom uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended.
+ *
+ * <p><ul>
+ * <li>2.004ms = full "forward"
+ * <li>1.520ms = the "high end" of the deadband range
+ * <li>1.500ms = center of the deadband range (off)
+ * <li>1.480ms = the "low end" of the deadband range
+ * <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class PWMVenom extends PWMSpeedController {
+ /**
+ * Constructor for a Venom connected via PWM.
+ *
+ * @param channel The PWM channel that the Venom is attached to. 0-9 are on-board, 10-19 are
+ * on the MXP port
+ */
+ public PWMVenom(final int channel) {
+ super(channel);
+
+ setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_FusionVenom, getChannel() + 1);
+ SendableRegistry.setName(this, "PWMVenom", getChannel());
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
index 94ad884..e2861f4 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
@@ -94,7 +94,11 @@
m_threadId = Thread.currentThread().getId();
setupCameraServerShared();
inst.setNetworkIdentity("Robot");
- inst.startServer("/home/lvuser/networktables.ini");
+ if (isReal()) {
+ inst.startServer("/home/lvuser/networktables.ini");
+ } else {
+ inst.startServer();
+ }
m_ds = DriverStation.getInstance();
inst.getTable("LiveWindow").getSubTable(".status").getEntry("LW Enabled").setBoolean(false);
@@ -317,7 +321,8 @@
// Needed because all the OpenCV JNI functions don't have built in loading
CameraServerJNI.enumerateSinks();
- HAL.report(tResourceType.kResourceType_Language, tInstances.kLanguage_Java);
+ HAL.report(tResourceType.kResourceType_Language, tInstances.kLanguage_Java, 0,
+ WPILibVersion.Version);
if (HAL.hasMain()) {
Thread thread = new Thread(() -> {
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
index 97d9b27..aca2cbe 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
@@ -424,6 +424,17 @@
return SPIJNI.spiGetAutoDroppedCount(m_port);
}
+ /**
+ * Configure the Auto SPI Stall time between reads.
+ *
+ * @param csToSclkTicks the number of ticks to wait before asserting the cs pin
+ * @param stallTicks the number of ticks to stall for
+ * @param pow2BytesPerRead the number of bytes to read before stalling
+ */
+ public void configureAutoStall(int csToSclkTicks, int stallTicks, int pow2BytesPerRead) {
+ SPIJNI.spiConfigureAutoStall(m_port, csToSclkTicks, stallTicks, pow2BytesPerRead);
+ }
+
private static final int kAccumulateDepth = 2048;
@SuppressWarnings("PMD.TooManyFields")
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java
new file mode 100644
index 0000000..2ba468b
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * A class that limits the rate of change of an input value. Useful for implementing voltage,
+ * setpoint, and/or output ramps. A slew-rate limit is most appropriate when the quantity being
+ * controlled is a velocity or a voltage; when controlling a position, consider using a
+ * {@link edu.wpi.first.wpilibj.trajectory.TrapezoidProfile} instead.
+ */
+public class SlewRateLimiter {
+ private final Timer m_timer = new Timer();
+ private final double m_rateLimit;
+ private double m_prevVal;
+
+ /**
+ * Creates a new SlewRateLimiter with the given rate limit and initial value.
+ *
+ * @param rateLimit The rate-of-change limit, in units per second.
+ * @param initialValue The initial value of the input.
+ */
+ public SlewRateLimiter(double rateLimit, double initialValue) {
+ m_prevVal = initialValue;
+ m_rateLimit = rateLimit;
+ m_timer.start();
+ }
+
+ /**
+ * Creates a new SlewRateLimiter with the given rate limit and an initial value of zero.
+ *
+ * @param rateLimit The rate-of-change limit, in units per second.
+ */
+ public SlewRateLimiter(double rateLimit) {
+ this(rateLimit, 0);
+ }
+
+ /**
+ * Filters the input to limit its slew rate.
+ *
+ * @param input The input value whose slew rate is to be limited.
+ * @return The filtered value, which will not change faster than the slew rate.
+ */
+ public double calculate(double input) {
+ m_prevVal += MathUtil.clamp(input - m_prevVal,
+ -m_rateLimit * m_timer.get(),
+ m_rateLimit * m_timer.get());
+ m_timer.reset();
+ return m_prevVal;
+ }
+
+ /**
+ * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
+ *
+ * @param value The value to reset to.
+ */
+ public void reset(double value) {
+ m_timer.reset();
+ m_prevVal = value;
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
index 355b182..33f9784 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
@@ -49,6 +49,7 @@
/**
* Calculates the feedforward from the gains and setpoints.
*
+ * @param positionRadians The position (angle) setpoint.
* @param velocityRadPerSec The velocity setpoint.
* @param accelRadPerSecSquared The acceleration setpoint.
* @return The computed feedforward.
@@ -64,7 +65,8 @@
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
* be zero).
*
- * @param velocity The velocity setpoint.
+ * @param positionRadians The position (angle) setpoint.
+ * @param velocity The velocity setpoint.
* @return The computed feedforward.
*/
public double calculate(double positionRadians, double velocity) {
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
index 04e34b1..eea0644 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -100,7 +100,7 @@
instances++;
SendableRegistry.addLW(this, "PIDController", instances);
- HAL.report(tResourceType.kResourceType_PIDController, instances);
+ HAL.report(tResourceType.kResourceType_PIDController2, instances);
}
@Override
@@ -328,7 +328,7 @@
}
/**
- * Resets the previous error and the integral term. Also disables the controller.
+ * Resets the previous error and the integral term.
*/
public void reset() {
m_prevError = 0;
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
index cc2f29d..7550aa5 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,16 +7,21 @@
package edu.wpi.first.wpilibj.controller;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
/**
* Implements a PID control loop whose setpoint is constrained by a trapezoid
- * profile.
+ * profile. Users should call reset() when they first start running the controller
+ * to avoid unwanted behavior.
*/
@SuppressWarnings("PMD.TooManyMethods")
public class ProfiledPIDController implements Sendable {
+ private static int instances;
+
private PIDController m_controller;
private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
@@ -54,6 +59,8 @@
double period) {
m_controller = new PIDController(Kp, Ki, Kd, period);
m_constraints = constraints;
+ instances++;
+ HAL.report(tResourceType.kResourceType_ProfiledPIDController, instances);
}
/**
@@ -313,10 +320,33 @@
}
/**
- * Reset the previous error, the integral term, and disable the controller.
+ * Reset the previous error and the integral term.
+ *
+ * @param measurement The current measured State of the system.
*/
- public void reset() {
+ public void reset(TrapezoidProfile.State measurement) {
m_controller.reset();
+ m_setpoint = measurement;
+ }
+
+ /**
+ * Reset the previous error and the integral term.
+ *
+ * @param measuredPosition The current measured position of the system.
+ * @param measuredVelocity The current measured velocity of the system.
+ */
+ public void reset(double measuredPosition, double measuredVelocity) {
+ reset(new TrapezoidProfile.State(measuredPosition, measuredVelocity));
+ }
+
+ /**
+ * Reset the previous error and the integral term.
+ *
+ * @param measuredPosition The current measured position of the system. The velocity is
+ * assumed to be zero.
+ */
+ public void reset(double measuredPosition) {
+ reset(measuredPosition, 0.0);
}
@Override
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
index e16f0a0..aa98a47 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
@@ -82,4 +82,10 @@
omegaRadiansPerSecond
);
}
+
+ @Override
+ public String toString() {
+ return String.format("ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)",
+ vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
+ }
}
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
index 06fdd32..bb632af 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
@@ -7,6 +7,10 @@
package edu.wpi.first.wpilibj.kinematics;
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
/**
* Helper class that converts a chassis velocity (dx and dtheta components) to
* left and right wheel velocities for a differential drive.
@@ -29,6 +33,7 @@
*/
public DifferentialDriveKinematics(double trackWidthMeters) {
this.trackWidthMeters = trackWidthMeters;
+ HAL.report(tResourceType.kResourceType_Kinematics, tInstances.kKinematics_DifferentialDrive);
}
/**
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
index 2a98f2b..35a4342 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
@@ -7,6 +7,9 @@
package edu.wpi.first.wpilibj.kinematics;
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Twist2d;
@@ -43,6 +46,7 @@
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
+ HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_DifferentialDrive);
}
/**
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
index 32c5ade..ac4a9fa 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
@@ -59,4 +59,10 @@
* attainableMaxSpeedMetersPerSecond;
}
}
+
+ @Override
+ public String toString() {
+ return String.format("DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)",
+ leftMetersPerSecond, rightMetersPerSecond);
+ }
}
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
index 9caad59..a5e5b5f 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
@@ -9,6 +9,9 @@
import org.ejml.simple.SimpleMatrix;
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.geometry.Translation2d;
/**
@@ -70,6 +73,8 @@
setInverseKinematics(frontLeftWheelMeters, frontRightWheelMeters,
rearLeftWheelMeters, rearRightWheelMeters);
m_forwardKinematics = m_inverseKinematics.pseudoInverse();
+
+ HAL.report(tResourceType.kResourceType_Kinematics, tInstances.kKinematics_MecanumDrive);
}
/**
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
index af4a7f1..f467d20 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
@@ -55,4 +55,11 @@
this.rearLeftVoltage = rearLeftVoltage;
this.rearRightVoltage = rearRightVoltage;
}
+
+ @Override
+ public String toString() {
+ return String.format("MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, "
+ + "Rear Left: %.2f V, Rear Right: %.2f V)", frontLeftVoltage, frontRightVoltage,
+ rearLeftVoltage, rearRightVoltage);
+ }
}
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
index 22ef83a..b3dd402 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
@@ -7,6 +7,9 @@
package edu.wpi.first.wpilibj.kinematics;
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -42,6 +45,7 @@
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
+ HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_MecanumDrive);
}
/**
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
index 54ae521..0f0dd71 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
@@ -81,4 +81,11 @@
* attainableMaxSpeedMetersPerSecond;
}
}
+
+ @Override
+ public String toString() {
+ return String.format("MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, "
+ + "Rear Left: %.2f m/s, Rear Right: %.2f m/s)", frontLeftMetersPerSecond,
+ frontRightMetersPerSecond, rearLeftMetersPerSecond, rearRightMetersPerSecond);
+ }
}
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java
index 0da2018..37f97b8 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java
@@ -12,6 +12,9 @@
import org.ejml.simple.SimpleMatrix;
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
@@ -69,6 +72,8 @@
m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX());
}
m_forwardKinematics = m_inverseKinematics.pseudoInverse();
+
+ HAL.report(tResourceType.kResourceType_Kinematics, tInstances.kKinematics_SwerveDrive);
}
/**
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
index c08070c..811c3de 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
@@ -7,6 +7,9 @@
package edu.wpi.first.wpilibj.kinematics;
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -42,6 +45,7 @@
m_poseMeters = initialPose;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPose.getRotation();
+ HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_SwerveDrive);
}
/**
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
index 528776f..e3119e5 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
@@ -54,4 +54,10 @@
public int compareTo(SwerveModuleState o) {
return Double.compare(this.speedMetersPerSecond, o.speedMetersPerSecond);
}
+
+ @Override
+ public String toString() {
+ return String.format("SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond,
+ angle);
+ }
}
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
index b78227d..4b7bf95 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -31,6 +31,7 @@
package edu.wpi.first.wpilibj.spline;
+import java.util.ArrayDeque;
import java.util.ArrayList;
import java.util.List;
@@ -43,6 +44,36 @@
private static final double kMaxDtheta = 0.0872;
/**
+ * A malformed spline does not actually explode the LIFO stack size. Instead, the stack size
+ * stays at a relatively small number (e.g. 30) and never decreases. Because of this, we must
+ * count iterations. Even long, complex paths don't usually go over 300 iterations, so hitting
+ * this maximum should definitely indicate something has gone wrong.
+ */
+ private static final int kMaxIterations = 5000;
+
+ @SuppressWarnings("MemberName")
+ private static class StackContents {
+ final double t1;
+ final double t0;
+
+ StackContents(double t0, double t1) {
+ this.t0 = t0;
+ this.t1 = t1;
+ }
+ }
+
+ public static class MalformedSplineException extends RuntimeException {
+ /**
+ * Create a new exception with the given message.
+ *
+ * @param message the message to pass with the exception
+ */
+ private MalformedSplineException(String message) {
+ super(message);
+ }
+ }
+
+ /**
* Private constructor because this is a utility class.
*/
private SplineParameterizer() {
@@ -53,7 +84,9 @@
* arcs until their dx, dy, and dtheta are within specific tolerances.
*
* @param spline The spline to parameterize.
- * @return A vector of poses and curvatures that represents various points on the spline.
+ * @return A list of poses and curvatures that represents various points on the spline.
+ * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+ * with approximately opposing headings)
*/
public static List<PoseWithCurvature> parameterize(Spline spline) {
return parameterize(spline, 0.0, 1.0);
@@ -66,41 +99,54 @@
* @param spline The spline to parameterize.
* @param t0 Starting internal spline parameter. It is recommended to use 0.0.
* @param t1 Ending internal spline parameter. It is recommended to use 1.0.
- * @return A vector of poses and curvatures that represents various points on the spline.
+ * @return A list of poses and curvatures that represents various points on the spline.
+ * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+ * with approximately opposing headings)
*/
+ @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
public static List<PoseWithCurvature> parameterize(Spline spline, double t0, double t1) {
- var arr = new ArrayList<PoseWithCurvature>();
+ var splinePoints = new ArrayList<PoseWithCurvature>();
- // The parameterization does not add the first initial point. Let's add
- // that.
- arr.add(spline.getPoint(t0));
+ // The parameterization does not add the initial point. Let's add that.
+ splinePoints.add(spline.getPoint(t0));
- getSegmentArc(spline, arr, t0, t1);
- return arr;
- }
+ // We use an "explicit stack" to simulate recursion, instead of a recursive function call
+ // This give us greater control, instead of a stack overflow
+ var stack = new ArrayDeque<StackContents>();
+ stack.push(new StackContents(t0, t1));
- /**
- * Breaks up the spline into arcs until the dx, dy, and theta of each arc is
- * within tolerance.
- *
- * @param spline The spline to parameterize.
- * @param vector Pointer to vector of poses.
- * @param t0 Starting point for arc.
- * @param t1 Ending point for arc.
- */
- private static void getSegmentArc(Spline spline, List<PoseWithCurvature> vector,
- double t0, double t1) {
- final var start = spline.getPoint(t0);
- final var end = spline.getPoint(t1);
+ StackContents current;
+ PoseWithCurvature start;
+ PoseWithCurvature end;
+ int iterations = 0;
- final var twist = start.poseMeters.log(end.poseMeters);
+ while (!stack.isEmpty()) {
+ current = stack.removeFirst();
+ start = spline.getPoint(current.t0);
+ end = spline.getPoint(current.t1);
- if (Math.abs(twist.dy) > kMaxDy || Math.abs(twist.dx) > kMaxDx
- || Math.abs(twist.dtheta) > kMaxDtheta) {
- getSegmentArc(spline, vector, t0, (t0 + t1) / 2);
- getSegmentArc(spline, vector, (t0 + t1) / 2, t1);
- } else {
- vector.add(spline.getPoint(t1));
+ final var twist = start.poseMeters.log(end.poseMeters);
+ if (
+ Math.abs(twist.dy) > kMaxDy
+ || Math.abs(twist.dx) > kMaxDx
+ || Math.abs(twist.dtheta) > kMaxDtheta
+ ) {
+ stack.addFirst(new StackContents((current.t0 + current.t1) / 2, current.t1));
+ stack.addFirst(new StackContents(current.t0, (current.t0 + current.t1) / 2));
+ } else {
+ splinePoints.add(spline.getPoint(current.t1));
+ }
+
+ iterations++;
+ if (iterations >= kMaxIterations) {
+ throw new MalformedSplineException(
+ "Could not parameterize a malformed spline. "
+ + "This means that you probably had two or more adjacent waypoints that were very close "
+ + "together with headings in opposing directions."
+ );
+ }
}
+
+ return splinePoints;
}
}
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
index 4f49244..f1d51b9 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
@@ -7,6 +7,7 @@
package edu.wpi.first.wpilibj.trajectory;
+import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import java.util.stream.Collectors;
@@ -14,6 +15,7 @@
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
/**
* Represents a time-parameterized trajectory. The trajectory contains of
@@ -61,6 +63,15 @@
}
/**
+ * Returns the initial pose of the trajectory.
+ *
+ * @return The initial pose of the trajectory.
+ */
+ public Pose2d getInitialPose() {
+ return sample(0).poseMeters;
+ }
+
+ /**
* Returns the overall duration of the trajectory.
*
* @return The duration of the trajectory.
@@ -133,6 +144,57 @@
}
/**
+ * Transforms all poses in the trajectory by the given transform. This is
+ * useful for converting a robot-relative trajectory into a field-relative
+ * trajectory. This works with respect to the first pose in the trajectory.
+ *
+ * @param transform The transform to transform the trajectory by.
+ * @return The transformed trajectory.
+ */
+ @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+ public Trajectory transformBy(Transform2d transform) {
+ var firstState = m_states.get(0);
+ var firstPose = firstState.poseMeters;
+
+ // Calculate the transformed first pose.
+ var newFirstPose = firstPose.plus(transform);
+ List<State> newStates = new ArrayList<>();
+
+ newStates.add(new State(
+ firstState.timeSeconds, firstState.velocityMetersPerSecond,
+ firstState.accelerationMetersPerSecondSq, newFirstPose, firstState.curvatureRadPerMeter
+ ));
+
+ for (int i = 1; i < m_states.size(); i++) {
+ var state = m_states.get(i);
+ // We are transforming relative to the coordinate frame of the new initial pose.
+ newStates.add(new State(
+ state.timeSeconds, state.velocityMetersPerSecond,
+ state.accelerationMetersPerSecondSq, newFirstPose.plus(state.poseMeters.minus(firstPose)),
+ state.curvatureRadPerMeter
+ ));
+ }
+
+ return new Trajectory(newStates);
+ }
+
+ /**
+ * Transforms all poses in the trajectory so that they are relative to the
+ * given pose. This is useful for converting a field-relative trajectory
+ * into a robot-relative trajectory.
+ *
+ * @param pose The pose that is the origin of the coordinate frame that
+ * the current trajectory will be transformed into.
+ * @return The transformed trajectory.
+ */
+ public Trajectory relativeTo(Pose2d pose) {
+ return new Trajectory(m_states.stream().map(state -> new State(state.timeSeconds,
+ state.velocityMetersPerSecond, state.accelerationMetersPerSecondSq,
+ state.poseMeters.relativeTo(pose), state.curvatureRadPerMeter))
+ .collect(Collectors.toList()));
+ }
+
+ /**
* Represents a time-parameterized trajectory. The trajectory contains of
* various States that represent the pose, curvature, time elapsed, velocity,
* and acceleration at that point.
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
index a4d6898..2e2ae7d 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,9 +8,11 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.ArrayList;
+import java.util.Arrays;
import java.util.Collection;
import java.util.List;
+import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
@@ -19,8 +21,12 @@
import edu.wpi.first.wpilibj.spline.Spline;
import edu.wpi.first.wpilibj.spline.SplineHelper;
import edu.wpi.first.wpilibj.spline.SplineParameterizer;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
public final class TrajectoryGenerator {
+ private static final Trajectory kDoNothingTrajectory =
+ new Trajectory(Arrays.asList(new Trajectory.State()));
+
/**
* Private constructor because this is a utility class.
*/
@@ -60,9 +66,14 @@
}
// Get the spline points
- var points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(
- newInitial, interiorWaypoints.toArray(new Translation2d[0]), newEnd
- ));
+ List<PoseWithCurvature> points;
+ try {
+ points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(newInitial,
+ interiorWaypoints.toArray(new Translation2d[0]), newEnd));
+ } catch (MalformedSplineException ex) {
+ DriverStation.reportError(ex.getMessage(), ex.getStackTrace());
+ return kDoNothingTrajectory;
+ }
// Change the points back to their original orientation.
if (config.isReversed()) {
@@ -130,9 +141,15 @@
}
// Get the spline points
- var points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors(
- newControlVectors.toArray(new Spline.ControlVector[]{})
- ));
+ List<PoseWithCurvature> points;
+ try {
+ points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors(
+ newControlVectors.toArray(new Spline.ControlVector[]{})
+ ));
+ } catch (MalformedSplineException ex) {
+ DriverStation.reportError(ex.getMessage(), ex.getStackTrace());
+ return kDoNothingTrajectory;
+ }
// Change the points back to their original orientation.
if (config.isReversed()) {
@@ -171,6 +188,8 @@
*
* @param splines The splines to parameterize.
* @return The spline points for use in time parameterization of a trajectory.
+ * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+ * with approximately opposing headings)
*/
public static List<PoseWithCurvature> splinePointsFromSplines(
Spline[] splines) {
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
index f4b1b12..0cd0f49 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -47,6 +47,7 @@
* @throws IOException if writing to the file fails
*/
public static void toPathweaverJson(Trajectory trajectory, Path path) throws IOException {
+ Files.createDirectories(path.getParent());
try (BufferedWriter writer = Files.newBufferedWriter(path)) {
WRITER.writeValue(writer, trajectory.getStates().toArray(new Trajectory.State[0]));
}
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
index 74ee846..0b24411 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
@@ -9,6 +9,9 @@
import java.util.Objects;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
/**
* A trapezoid-shaped velocity profile.
*
@@ -59,11 +62,19 @@
public double maxAcceleration;
public Constraints() {
+ HAL.report(tResourceType.kResourceType_TrapezoidProfile, 1);
}
+ /**
+ * Construct constraints for a TrapezoidProfile.
+ *
+ * @param maxVelocity maximum velocity
+ * @param maxAcceleration maximum acceleration
+ */
public Constraints(double maxVelocity, double maxAcceleration) {
this.maxVelocity = maxVelocity;
this.maxAcceleration = maxAcceleration;
+ HAL.report(tResourceType.kResourceType_TrapezoidProfile, 1);
}
}
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java
index d567009..8ca63f4 100644
--- a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java
@@ -22,7 +22,7 @@
private final SwerveDriveKinematics m_kinematics;
/**
- * Constructs a mecanum drive dynamics constraint.
+ * Constructs a swerve drive dynamics constraint.
*
* @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
*/
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
new file mode 100644
index 0000000..e4d5b69
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
@@ -0,0 +1,798 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+import java.util.Objects;
+
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * Represents colors.
+ *
+ * <p>Limited to 12 bits of precision.
+ */
+@SuppressWarnings("MemberName")
+public class Color {
+ /*
+ * FIRST Colors
+ */
+
+ /**
+ * #1560BD.
+ */
+ public static final Color kDenim = new Color(0.0823529412, 0.376470589, 0.7411764706);
+
+ /**
+ * #0066B3.
+ */
+ public static final Color kFirstBlue = new Color(0.0, 0.4, 0.7019607844);
+
+ /**
+ * #ED1C24.
+ */
+ public static final Color kFirstRed = new Color(0.9294117648, 0.1098039216, 0.1411764706);
+
+ /*
+ * Standard Colors
+ */
+
+ /**
+ * #F0F8FF.
+ */
+ public static final Color kAliceBlue = new Color(0.9411765f, 0.972549f, 1.0f);
+
+ /**
+ * #FAEBD7.
+ */
+ public static final Color kAntiqueWhite = new Color(0.98039216f, 0.92156863f, 0.84313726f);
+
+ /**
+ * #00FFFF.
+ */
+ public static final Color kAqua = new Color(0.0f, 1.0f, 1.0f);
+
+ /**
+ * #7FFFD4.
+ */
+ public static final Color kAquamarine = new Color(0.49803922f, 1.0f, 0.83137256f);
+
+ /**
+ * #F0FFFF.
+ */
+ public static final Color kAzure = new Color(0.9411765f, 1.0f, 1.0f);
+
+ /**
+ * #F5F5DC.
+ */
+ public static final Color kBeige = new Color(0.9607843f, 0.9607843f, 0.8627451f);
+
+ /**
+ * #FFE4C4.
+ */
+ public static final Color kBisque = new Color(1.0f, 0.89411765f, 0.76862746f);
+
+ /**
+ * #000000.
+ */
+ public static final Color kBlack = new Color(0.0f, 0.0f, 0.0f);
+
+ /**
+ * #FFEBCD.
+ */
+ public static final Color kBlanchedAlmond = new Color(1.0f, 0.92156863f, 0.8039216f);
+
+ /**
+ * #0000FF.
+ */
+ public static final Color kBlue = new Color(0.0f, 0.0f, 1.0f);
+
+ /**
+ * #8A2BE2.
+ */
+ public static final Color kBlueViolet = new Color(0.5411765f, 0.16862746f, 0.8862745f);
+
+ /**
+ * #A52A2A.
+ */
+ public static final Color kBrown = new Color(0.64705884f, 0.16470589f, 0.16470589f);
+
+ /**
+ * #DEB887.
+ */
+ public static final Color kBurlywood = new Color(0.87058824f, 0.72156864f, 0.5294118f);
+
+ /**
+ * #5F9EA0.
+ */
+ public static final Color kCadetBlue = new Color(0.37254903f, 0.61960787f, 0.627451f);
+
+ /**
+ * #7FFF00.
+ */
+ public static final Color kChartreuse = new Color(0.49803922f, 1.0f, 0.0f);
+
+ /**
+ * #D2691E.
+ */
+ public static final Color kChocolate = new Color(0.8235294f, 0.4117647f, 0.11764706f);
+
+ /**
+ * #FF7F50.
+ */
+ public static final Color kCoral = new Color(1.0f, 0.49803922f, 0.3137255f);
+
+ /**
+ * #6495ED.
+ */
+ public static final Color kCornflowerBlue = new Color(0.39215687f, 0.58431375f, 0.92941177f);
+
+ /**
+ * #FFF8DC.
+ */
+ public static final Color kCornsilk = new Color(1.0f, 0.972549f, 0.8627451f);
+
+ /**
+ * #DC143C.
+ */
+ public static final Color kCrimson = new Color(0.8627451f, 0.078431375f, 0.23529412f);
+
+ /**
+ * #00FFFF.
+ */
+ public static final Color kCyan = new Color(0.0f, 1.0f, 1.0f);
+
+ /**
+ * #00008B.
+ */
+ public static final Color kDarkBlue = new Color(0.0f, 0.0f, 0.54509807f);
+
+ /**
+ * #008B8B.
+ */
+ public static final Color kDarkCyan = new Color(0.0f, 0.54509807f, 0.54509807f);
+
+ /**
+ * #B8860B.
+ */
+ public static final Color kDarkGoldenrod = new Color(0.72156864f, 0.5254902f, 0.043137256f);
+
+ /**
+ * #A9A9A9.
+ */
+ public static final Color kDarkGray = new Color(0.6627451f, 0.6627451f, 0.6627451f);
+
+ /**
+ * #006400.
+ */
+ public static final Color kDarkGreen = new Color(0.0f, 0.39215687f, 0.0f);
+
+ /**
+ * #BDB76B.
+ */
+ public static final Color kDarkKhaki = new Color(0.7411765f, 0.7176471f, 0.41960785f);
+
+ /**
+ * #8B008B.
+ */
+ public static final Color kDarkMagenta = new Color(0.54509807f, 0.0f, 0.54509807f);
+
+ /**
+ * #556B2F.
+ */
+ public static final Color kDarkOliveGreen = new Color(0.33333334f, 0.41960785f, 0.18431373f);
+
+ /**
+ * #FF8C00.
+ */
+ public static final Color kDarkOrange = new Color(1.0f, 0.54901963f, 0.0f);
+
+ /**
+ * #9932CC.
+ */
+ public static final Color kDarkOrchid = new Color(0.6f, 0.19607843f, 0.8f);
+
+ /**
+ * #8B0000.
+ */
+ public static final Color kDarkRed = new Color(0.54509807f, 0.0f, 0.0f);
+
+ /**
+ * #E9967A.
+ */
+ public static final Color kDarkSalmon = new Color(0.9137255f, 0.5882353f, 0.47843137f);
+
+ /**
+ * #8FBC8F.
+ */
+ public static final Color kDarkSeaGreen = new Color(0.56078434f, 0.7372549f, 0.56078434f);
+
+ /**
+ * #483D8B.
+ */
+ public static final Color kDarkSlateBlue = new Color(0.28235295f, 0.23921569f, 0.54509807f);
+
+ /**
+ * #2F4F4F.
+ */
+ public static final Color kDarkSlateGray = new Color(0.18431373f, 0.30980393f, 0.30980393f);
+
+ /**
+ * #00CED1.
+ */
+ public static final Color kDarkTurquoise = new Color(0.0f, 0.80784315f, 0.81960785f);
+
+ /**
+ * #9400D3.
+ */
+ public static final Color kDarkViolet = new Color(0.5803922f, 0.0f, 0.827451f);
+
+ /**
+ * #FF1493.
+ */
+ public static final Color kDeepPink = new Color(1.0f, 0.078431375f, 0.5764706f);
+
+ /**
+ * #00BFFF.
+ */
+ public static final Color kDeepSkyBlue = new Color(0.0f, 0.7490196f, 1.0f);
+
+ /**
+ * #696969.
+ */
+ public static final Color kDimGray = new Color(0.4117647f, 0.4117647f, 0.4117647f);
+
+ /**
+ * #1E90FF.
+ */
+ public static final Color kDodgerBlue = new Color(0.11764706f, 0.5647059f, 1.0f);
+
+ /**
+ * #B22222.
+ */
+ public static final Color kFirebrick = new Color(0.69803923f, 0.13333334f, 0.13333334f);
+
+ /**
+ * #FFFAF0.
+ */
+ public static final Color kFloralWhite = new Color(1.0f, 0.98039216f, 0.9411765f);
+
+ /**
+ * #228B22.
+ */
+ public static final Color kForestGreen = new Color(0.13333334f, 0.54509807f, 0.13333334f);
+
+ /**
+ * #FF00FF.
+ */
+ public static final Color kFuchsia = new Color(1.0f, 0.0f, 1.0f);
+
+ /**
+ * #DCDCDC.
+ */
+ public static final Color kGainsboro = new Color(0.8627451f, 0.8627451f, 0.8627451f);
+
+ /**
+ * #F8F8FF.
+ */
+ public static final Color kGhostWhite = new Color(0.972549f, 0.972549f, 1.0f);
+
+ /**
+ * #FFD700.
+ */
+ public static final Color kGold = new Color(1.0f, 0.84313726f, 0.0f);
+
+ /**
+ * #DAA520.
+ */
+ public static final Color kGoldenrod = new Color(0.85490197f, 0.64705884f, 0.1254902f);
+
+ /**
+ * #808080.
+ */
+ public static final Color kGray = new Color(0.5019608f, 0.5019608f, 0.5019608f);
+
+ /**
+ * #008000.
+ */
+ public static final Color kGreen = new Color(0.0f, 0.5019608f, 0.0f);
+
+ /**
+ * #ADFF2F.
+ */
+ public static final Color kGreenYellow = new Color(0.6784314f, 1.0f, 0.18431373f);
+
+ /**
+ * #F0FFF0.
+ */
+ public static final Color kHoneydew = new Color(0.9411765f, 1.0f, 0.9411765f);
+
+ /**
+ * #FF69B4.
+ */
+ public static final Color kHotPink = new Color(1.0f, 0.4117647f, 0.7058824f);
+
+ /**
+ * #CD5C5C.
+ */
+ public static final Color kIndianRed = new Color(0.8039216f, 0.36078432f, 0.36078432f);
+
+ /**
+ * #4B0082.
+ */
+ public static final Color kIndigo = new Color(0.29411766f, 0.0f, 0.50980395f);
+
+ /**
+ * #FFFFF0.
+ */
+ public static final Color kIvory = new Color(1.0f, 1.0f, 0.9411765f);
+
+ /**
+ * #F0E68C.
+ */
+ public static final Color kKhaki = new Color(0.9411765f, 0.9019608f, 0.54901963f);
+
+ /**
+ * #E6E6FA.
+ */
+ public static final Color kLavender = new Color(0.9019608f, 0.9019608f, 0.98039216f);
+
+ /**
+ * #FFF0F5.
+ */
+ public static final Color kLavenderBlush = new Color(1.0f, 0.9411765f, 0.9607843f);
+
+ /**
+ * #7CFC00.
+ */
+ public static final Color kLawnGreen = new Color(0.4862745f, 0.9882353f, 0.0f);
+
+ /**
+ * #FFFACD.
+ */
+ public static final Color kLemonChiffon = new Color(1.0f, 0.98039216f, 0.8039216f);
+
+ /**
+ * #ADD8E6.
+ */
+ public static final Color kLightBlue = new Color(0.6784314f, 0.84705883f, 0.9019608f);
+
+ /**
+ * #F08080.
+ */
+ public static final Color kLightCoral = new Color(0.9411765f, 0.5019608f, 0.5019608f);
+
+ /**
+ * #E0FFFF.
+ */
+ public static final Color kLightCyan = new Color(0.8784314f, 1.0f, 1.0f);
+
+ /**
+ * #FAFAD2.
+ */
+ public static final Color kLightGoldenrodYellow = new Color(0.98039216f, 0.98039216f, 0.8235294f);
+
+ /**
+ * #D3D3D3.
+ */
+ public static final Color kLightGray = new Color(0.827451f, 0.827451f, 0.827451f);
+
+ /**
+ * #90EE90.
+ */
+ public static final Color kLightGreen = new Color(0.5647059f, 0.93333334f, 0.5647059f);
+
+ /**
+ * #FFB6C1.
+ */
+ public static final Color kLightPink = new Color(1.0f, 0.7137255f, 0.75686276f);
+
+ /**
+ * #FFA07A.
+ */
+ public static final Color kLightSalmon = new Color(1.0f, 0.627451f, 0.47843137f);
+
+ /**
+ * #20B2AA.
+ */
+ public static final Color kLightSeagGeen = new Color(0.1254902f, 0.69803923f, 0.6666667f);
+
+ /**
+ * #87CEFA.
+ */
+ public static final Color kLightSkyBlue = new Color(0.5294118f, 0.80784315f, 0.98039216f);
+
+ /**
+ * #778899.
+ */
+ public static final Color kLightSlateGray = new Color(0.46666667f, 0.53333336f, 0.6f);
+
+ /**
+ * #B0C4DE.
+ */
+ public static final Color kLightSteellue = new Color(0.6901961f, 0.76862746f, 0.87058824f);
+
+ /**
+ * #FFFFE0.
+ */
+ public static final Color kLightYellow = new Color(1.0f, 1.0f, 0.8784314f);
+
+ /**
+ * #00FF00.
+ */
+ public static final Color kLime = new Color(0.0f, 1.0f, 0.0f);
+
+ /**
+ * #32CD32.
+ */
+ public static final Color kLimeGreen = new Color(0.19607843f, 0.8039216f, 0.19607843f);
+
+ /**
+ * #FAF0E6.
+ */
+ public static final Color kLinen = new Color(0.98039216f, 0.9411765f, 0.9019608f);
+
+ /**
+ * #FF00FF.
+ */
+ public static final Color kMagenta = new Color(1.0f, 0.0f, 1.0f);
+
+ /**
+ * #800000.
+ */
+ public static final Color kMaroon = new Color(0.5019608f, 0.0f, 0.0f);
+
+ /**
+ * #66CDAA.
+ */
+ public static final Color kMediumAquamarine = new Color(0.4f, 0.8039216f, 0.6666667f);
+
+ /**
+ * #0000CD.
+ */
+ public static final Color kMediumBlue = new Color(0.0f, 0.0f, 0.8039216f);
+
+ /**
+ * #BA55D3.
+ */
+ public static final Color kMediumOrchid = new Color(0.7294118f, 0.33333334f, 0.827451f);
+
+ /**
+ * #9370DB.
+ */
+ public static final Color kMediumPurple = new Color(0.5764706f, 0.4392157f, 0.85882354f);
+
+ /**
+ * #3CB371.
+ */
+ public static final Color kMediumSeaGreen = new Color(0.23529412f, 0.7019608f, 0.44313726f);
+
+ /**
+ * #7B68EE.
+ */
+ public static final Color kMediumSlateBlue = new Color(0.48235294f, 0.40784314f, 0.93333334f);
+
+ /**
+ * #00FA9A.
+ */
+ public static final Color kMediumSpringGreen = new Color(0.0f, 0.98039216f, 0.6039216f);
+
+ /**
+ * #48D1CC.
+ */
+ public static final Color kMediumTurquoise = new Color(0.28235295f, 0.81960785f, 0.8f);
+
+ /**
+ * #C71585.
+ */
+ public static final Color kMediumVioletRed = new Color(0.78039217f, 0.08235294f, 0.52156866f);
+
+ /**
+ * #191970.
+ */
+ public static final Color kMidnightBlue = new Color(0.09803922f, 0.09803922f, 0.4392157f);
+
+ /**
+ * #F5FFFA.
+ */
+ public static final Color kMintcream = new Color(0.9607843f, 1.0f, 0.98039216f);
+
+ /**
+ * #FFE4E1.
+ */
+ public static final Color kMistyRose = new Color(1.0f, 0.89411765f, 0.88235295f);
+
+ /**
+ * #FFE4B5.
+ */
+ public static final Color kMoccasin = new Color(1.0f, 0.89411765f, 0.70980394f);
+
+ /**
+ * #FFDEAD.
+ */
+ public static final Color kNavajoWhite = new Color(1.0f, 0.87058824f, 0.6784314f);
+
+ /**
+ * #000080.
+ */
+ public static final Color kNavy = new Color(0.0f, 0.0f, 0.5019608f);
+
+ /**
+ * #FDF5E6.
+ */
+ public static final Color kOldLace = new Color(0.99215686f, 0.9607843f, 0.9019608f);
+
+ /**
+ * #808000.
+ */
+ public static final Color kOlive = new Color(0.5019608f, 0.5019608f, 0.0f);
+
+ /**
+ * #6B8E23.
+ */
+ public static final Color kOliveDrab = new Color(0.41960785f, 0.5568628f, 0.13725491f);
+
+ /**
+ * #FFA500.
+ */
+ public static final Color kOrange = new Color(1.0f, 0.64705884f, 0.0f);
+
+ /**
+ * #FF4500.
+ */
+ public static final Color kOrangeRed = new Color(1.0f, 0.27058825f, 0.0f);
+
+ /**
+ * #DA70D6.
+ */
+ public static final Color kOrchid = new Color(0.85490197f, 0.4392157f, 0.8392157f);
+
+ /**
+ * #EEE8AA.
+ */
+ public static final Color kPaleGoldenrod = new Color(0.93333334f, 0.9098039f, 0.6666667f);
+
+ /**
+ * #98FB98.
+ */
+ public static final Color kPaleGreen = new Color(0.59607846f, 0.9843137f, 0.59607846f);
+
+ /**
+ * #AFEEEE.
+ */
+ public static final Color kPaleTurquoise = new Color(0.6862745f, 0.93333334f, 0.93333334f);
+
+ /**
+ * #DB7093.
+ */
+ public static final Color kPaleVioletRed = new Color(0.85882354f, 0.4392157f, 0.5764706f);
+
+ /**
+ * #FFEFD5.
+ */
+ public static final Color kPapayaWhip = new Color(1.0f, 0.9372549f, 0.8352941f);
+
+ /**
+ * #FFDAB9.
+ */
+ public static final Color kPeachPuff = new Color(1.0f, 0.85490197f, 0.7254902f);
+
+ /**
+ * #CD853F.
+ */
+ public static final Color kPeru = new Color(0.8039216f, 0.52156866f, 0.24705882f);
+
+ /**
+ * #FFC0CB.
+ */
+ public static final Color kPink = new Color(1.0f, 0.7529412f, 0.79607844f);
+
+ /**
+ * #DDA0DD.
+ */
+ public static final Color kPlum = new Color(0.8666667f, 0.627451f, 0.8666667f);
+
+ /**
+ * #B0E0E6.
+ */
+ public static final Color kPowderBlue = new Color(0.6901961f, 0.8784314f, 0.9019608f);
+
+ /**
+ * #800080.
+ */
+ public static final Color kPurple = new Color(0.5019608f, 0.0f, 0.5019608f);
+
+ /**
+ * #FF0000.
+ */
+ public static final Color kRed = new Color(1.0f, 0.0f, 0.0f);
+
+ /**
+ * #BC8F8F.
+ */
+ public static final Color kRosyBrown = new Color(0.7372549f, 0.56078434f, 0.56078434f);
+
+ /**
+ * #4169E1.
+ */
+ public static final Color kRoyalBlue = new Color(0.25490198f, 0.4117647f, 0.88235295f);
+
+ /**
+ * #8B4513.
+ */
+ public static final Color kSaddleBrown = new Color(0.54509807f, 0.27058825f, 0.07450981f);
+
+ /**
+ * #FA8072.
+ */
+ public static final Color kSalmon = new Color(0.98039216f, 0.5019608f, 0.44705883f);
+
+ /**
+ * #F4A460.
+ */
+ public static final Color kSandyBrown = new Color(0.95686275f, 0.6431373f, 0.3764706f);
+
+ /**
+ * #2E8B57.
+ */
+ public static final Color kSeaGreen = new Color(0.18039216f, 0.54509807f, 0.34117648f);
+
+ /**
+ * #FFF5EE.
+ */
+ public static final Color kSeashell = new Color(1.0f, 0.9607843f, 0.93333334f);
+
+ /**
+ * #A0522D.
+ */
+ public static final Color kSienna = new Color(0.627451f, 0.32156864f, 0.1764706f);
+
+ /**
+ * #C0C0C0.
+ */
+ public static final Color kSilver = new Color(0.7529412f, 0.7529412f, 0.7529412f);
+
+ /**
+ * #87CEEB.
+ */
+ public static final Color kSkyBlue = new Color(0.5294118f, 0.80784315f, 0.92156863f);
+
+ /**
+ * #6A5ACD.
+ */
+ public static final Color kSlateBlue = new Color(0.41568628f, 0.3529412f, 0.8039216f);
+
+ /**
+ * #708090.
+ */
+ public static final Color kSlateGray = new Color(0.4392157f, 0.5019608f, 0.5647059f);
+
+ /**
+ * #FFFAFA.
+ */
+ public static final Color kSnow = new Color(1.0f, 0.98039216f, 0.98039216f);
+
+ /**
+ * #00FF7F.
+ */
+ public static final Color kSpringGreen = new Color(0.0f, 1.0f, 0.49803922f);
+
+ /**
+ * #4682B4.
+ */
+ public static final Color kSteelBlue = new Color(0.27450982f, 0.50980395f, 0.7058824f);
+
+ /**
+ * #D2B48C.
+ */
+ public static final Color kTan = new Color(0.8235294f, 0.7058824f, 0.54901963f);
+
+ /**
+ * #008080.
+ */
+ public static final Color kTeal = new Color(0.0f, 0.5019608f, 0.5019608f);
+
+ /**
+ * #D8BFD8.
+ */
+ public static final Color kThistle = new Color(0.84705883f, 0.7490196f, 0.84705883f);
+
+ /**
+ * #FF6347.
+ */
+ public static final Color kTomato = new Color(1.0f, 0.3882353f, 0.2784314f);
+
+ /**
+ * #40E0D0.
+ */
+ public static final Color kTurquoise = new Color(0.2509804f, 0.8784314f, 0.8156863f);
+
+ /**
+ * #EE82EE.
+ */
+ public static final Color kViolet = new Color(0.93333334f, 0.50980395f, 0.93333334f);
+
+ /**
+ * #F5DEB3.
+ */
+ public static final Color kWheat = new Color(0.9607843f, 0.87058824f, 0.7019608f);
+
+ /**
+ * #FFFFFF.
+ */
+ public static final Color kWhite = new Color(1.0f, 1.0f, 1.0f);
+
+ /**
+ * #F5F5F5.
+ */
+ public static final Color kWhiteSmoke = new Color(0.9607843f, 0.9607843f, 0.9607843f);
+
+ /**
+ * #FFFF00.
+ */
+ public static final Color kYellow = new Color(1.0f, 1.0f, 0.0f);
+
+ /**
+ * #9ACD32.
+ */
+ public static final Color kYellowGreen = new Color(0.6039216f, 0.8039216f, 0.19607843f);
+
+ public final double red;
+ public final double green;
+ public final double blue;
+
+ private static final double kPrecision = Math.pow(2, -12);
+
+ /**
+ * Constructs a Color.
+ *
+ * @param red Red value (0-1)
+ * @param green Green value (0-1)
+ * @param blue Blue value (0-1)
+ */
+ Color(double red, double green, double blue) {
+ this.red = roundAndClamp(red);
+ this.green = roundAndClamp(green);
+ this.blue = roundAndClamp(blue);
+ }
+
+ /**
+ * Constructs a Color from a Color8Bit.
+ *
+ * @param color The color
+ */
+ public Color(Color8Bit color) {
+ this(color.red / 255.0,
+ color.green / 255.0,
+ color.blue / 255.0);
+ }
+
+ @Override
+ public boolean equals(Object other) {
+ if (this == other) {
+ return true;
+ }
+ if (other == null || getClass() != other.getClass()) {
+ return false;
+ }
+
+ Color color = (Color) other;
+ return Double.compare(color.red, red) == 0
+ && Double.compare(color.green, green) == 0
+ && Double.compare(color.blue, blue) == 0;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(red, green, blue);
+ }
+
+ private static double roundAndClamp(double value) {
+ final var rounded = Math.round(value / kPrecision) * kPrecision;
+ return MathUtil.clamp(rounded, 0.0, 1.0);
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java
new file mode 100644
index 0000000..50d42fc
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+import java.util.Objects;
+
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * Represents colors with 8 bits of precision.
+ */
+@SuppressWarnings("MemberName")
+public class Color8Bit {
+ public final int red;
+ public final int green;
+ public final int blue;
+
+ /**
+ * Constructs a Color8Bit.
+ *
+ * @param red Red value (0-255)
+ * @param green Green value (0-255)
+ * @param blue Blue value (0-255)
+ */
+ public Color8Bit(int red, int green, int blue) {
+ this.red = MathUtil.clamp(red, 0, 255);
+ this.green = MathUtil.clamp(green, 0, 255);
+ this.blue = MathUtil.clamp(blue, 0, 255);
+ }
+
+ /**
+ * Constructs a Color8Bit from a Color.
+ *
+ * @param color The color
+ */
+ public Color8Bit(Color color) {
+ this((int) (color.red * 255),
+ (int) (color.green * 255),
+ (int) (color.blue * 255));
+ }
+
+ @Override
+ public boolean equals(Object other) {
+ if (this == other) {
+ return true;
+ }
+ if (other == null || getClass() != other.getClass()) {
+ return false;
+ }
+
+ Color8Bit color8Bit = (Color8Bit) other;
+ return red == color8Bit.red
+ && green == color8Bit.green
+ && blue == color8Bit.blue;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(red, green, blue);
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java b/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java
new file mode 100644
index 0000000..e47b70b
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+public class SlewRateLimiterTest {
+ @Test
+ void slewRateLimitTest() {
+ SlewRateLimiter limiter = new SlewRateLimiter(1);
+ Timer.delay(1);
+ assertTrue(limiter.calculate(2) < 2);
+ }
+
+ @Test
+ void slewRateNoLimitTest() {
+ SlewRateLimiter limiter = new SlewRateLimiter(1);
+ Timer.delay(1);
+ assertEquals(limiter.calculate(0.5), 0.5);
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java b/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java
new file mode 100644
index 0000000..6babcc2
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class ProfiledPIDControllerTest {
+ @Test
+ void testStartFromNonZeroPosition() {
+ ProfiledPIDController controller = new ProfiledPIDController(1.0, 0.0, 0.0,
+ new TrapezoidProfile.Constraints(1.0, 1.0));
+
+ controller.reset(20);
+
+ assertEquals(0.0, controller.calculate(20), 0.05);
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java b/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
index 5a8349d..45a0638 100644
--- a/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
+++ b/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,6 +8,7 @@
package edu.wpi.first.wpilibj.spline;
import java.util.ArrayList;
+import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
@@ -15,9 +16,11 @@
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -145,4 +148,15 @@
run(start, waypoints, end);
}
+
+ @Test
+ void testMalformed() {
+ assertThrows(MalformedSplineException.class, () -> run(
+ new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+ new ArrayList<>(), new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
+ assertThrows(MalformedSplineException.class, () -> run(
+ new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
+ Arrays.asList(new Translation2d(10, 10.5)),
+ new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
+ }
}
diff --git a/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java b/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java
index 5eb8437..1d65922 100644
--- a/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java
+++ b/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,9 +13,11 @@
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.assertTrue;
class QuinticHermiteSplineTest {
@@ -23,7 +25,7 @@
private static final double kMaxDy = 0.00127;
private static final double kMaxDtheta = 0.0872;
- @SuppressWarnings({"ParameterName", "PMD.UnusedLocalVariable"})
+ @SuppressWarnings({ "ParameterName", "PMD.UnusedLocalVariable" })
private void run(Pose2d a, Pose2d b) {
// Start the timer.
var start = System.nanoTime();
@@ -49,29 +51,27 @@
assertAll(
() -> assertTrue(Math.abs(twist.dx) < kMaxDx),
() -> assertTrue(Math.abs(twist.dy) < kMaxDy),
- () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta)
- );
+ () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta));
}
// Check first point
assertAll(
- () -> assertEquals(a.getTranslation().getX(),
- poses.get(0).poseMeters.getTranslation().getX(), 1E-9),
- () -> assertEquals(a.getTranslation().getY(),
- poses.get(0).poseMeters.getTranslation().getY(), 1E-9),
- () -> assertEquals(a.getRotation().getRadians(),
- poses.get(0).poseMeters.getRotation().getRadians(), 1E-9)
- );
+ () -> assertEquals(
+ a.getTranslation().getX(), poses.get(0).poseMeters.getTranslation().getX(), 1E-9),
+ () -> assertEquals(
+ a.getTranslation().getY(), poses.get(0).poseMeters.getTranslation().getY(), 1E-9),
+ () -> assertEquals(
+ a.getRotation().getRadians(), poses.get(0).poseMeters.getRotation().getRadians(),
+ 1E-9));
// Check last point
assertAll(
- () -> assertEquals(b.getTranslation().getX(),
- poses.get(poses.size() - 1).poseMeters.getTranslation().getX(), 1E-9),
- () -> assertEquals(b.getTranslation().getY(),
- poses.get(poses.size() - 1).poseMeters.getTranslation().getY(), 1E-9),
+ () -> assertEquals(b.getTranslation().getX(), poses.get(poses.size() - 1)
+ .poseMeters.getTranslation().getX(), 1E-9),
+ () -> assertEquals(b.getTranslation().getY(), poses.get(poses.size() - 1)
+ .poseMeters.getTranslation().getY(), 1E-9),
() -> assertEquals(b.getRotation().getRadians(),
- poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9)
- );
+ poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9));
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@@ -89,7 +89,20 @@
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testSquiggly() {
- run(new Pose2d(0, 0, Rotation2d.fromDegrees(90)),
+ run(
+ new Pose2d(0, 0, Rotation2d.fromDegrees(90)),
new Pose2d(-1, 0, Rotation2d.fromDegrees(90)));
}
+
+ @Test
+ void testMalformed() {
+ assertThrows(MalformedSplineException.class,
+ () -> run(
+ new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+ new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
+ assertThrows(MalformedSplineException.class,
+ () -> run(
+ new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
+ new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
+ }
}
diff --git a/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java b/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
index 5b45e3e..81fb5b4 100644
--- a/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
+++ b/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,10 +8,12 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.ArrayList;
+import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
+import edu.wpi.first.hal.sim.DriverStationSim;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
@@ -20,6 +22,7 @@
import static edu.wpi.first.wpilibj.util.Units.feetToMeters;
import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
class TrajectoryGeneratorTest {
@@ -69,4 +72,24 @@
);
}
}
+
+ @Test
+ void testMalformedTrajectory() {
+ var dsSim = new DriverStationSim();
+ dsSim.setSendError(false);
+
+ var traj =
+ TrajectoryGenerator.generateTrajectory(
+ Arrays.asList(
+ new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+ new Pose2d(1, 0, Rotation2d.fromDegrees(180))
+ ),
+ new TrajectoryConfig(feetToMeters(12), feetToMeters(12))
+ );
+
+ assertEquals(traj.getStates().size(), 1);
+ assertEquals(traj.getTotalTimeSeconds(), 0);
+
+ dsSim.setSendError(true);
+ }
}
diff --git a/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java b/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java
new file mode 100644
index 0000000..19f31dc
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class TrajectoryTransformTest {
+ @Test
+ void testTransformBy() {
+ var config = new TrajectoryConfig(3, 3);
+ var trajectory = TrajectoryGenerator.generateTrajectory(
+ new Pose2d(), List.of(), new Pose2d(1, 1, Rotation2d.fromDegrees(90)),
+ config
+ );
+
+ var transformedTrajectory = trajectory.transformBy(
+ new Transform2d(new Translation2d(1, 2), Rotation2d.fromDegrees(30)));
+
+ // Test initial pose.
+ assertEquals(new Pose2d(1, 2, Rotation2d.fromDegrees(30)),
+ transformedTrajectory.sample(0).poseMeters);
+
+ testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
+ }
+
+ @Test
+ void testRelativeTo() {
+ var config = new TrajectoryConfig(3, 3);
+ var trajectory = TrajectoryGenerator.generateTrajectory(
+ new Pose2d(1, 2, Rotation2d.fromDegrees(30.0)),
+ List.of(), new Pose2d(5, 7, Rotation2d.fromDegrees(90)),
+ config
+ );
+
+ var transformedTrajectory = trajectory.relativeTo(new Pose2d(1, 2, Rotation2d.fromDegrees(30)));
+
+ // Test initial pose.
+ assertEquals(new Pose2d(), transformedTrajectory.sample(0).poseMeters);
+
+ testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
+ }
+
+ void testSameShapedTrajectory(List<Trajectory.State> statesA, List<Trajectory.State> statesB) {
+ for (int i = 0; i < statesA.size() - 1; i++) {
+ var a1 = statesA.get(i).poseMeters;
+ var a2 = statesA.get(i + 1).poseMeters;
+
+ var b1 = statesB.get(i).poseMeters;
+ var b2 = statesB.get(i + 1).poseMeters;
+
+ assertEquals(a2.relativeTo(a1), b2.relativeTo(b1));
+ }
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/build.gradle b/third_party/allwpilib_2019/wpilibjExamples/build.gradle
index 651d8e5..2016632 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/build.gradle
+++ b/third_party/allwpilib_2019/wpilibjExamples/build.gradle
@@ -39,6 +39,10 @@
}
}
+tasks.register('buildDesktopJava') {
+ it.dependsOn tasks.withType(JavaCompile)
+}
+
apply from: 'publish.gradle'
ext {
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command2/ReplaceMeCommand.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command2/ReplaceMeCommand.java
new file mode 100644
index 0000000..5d3a501
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command2/ReplaceMeCommand.java
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.commands.command2;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class ReplaceMeCommand extends CommandBase {
+ /**
+ * Creates a new ReplaceMeCommand.
+ */
+ public ReplaceMeCommand() {
+ // Use addRequirements() here to declare subsystem dependencies.
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json
index 2894926..a26d15b 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json
@@ -6,70 +6,208 @@
"class"
],
"foldername": "emptyclass",
- "replacename": "ReplaceMeEmptyClass"
+ "replacename": "ReplaceMeEmptyClass",
+ "commandversion": 0
},
{
- "name": "Command",
+ "name": "Command (Old)",
"description": "Create a base command",
"tags": [
"command"
],
"foldername": "command",
- "replacename": "ReplaceMeCommand"
+ "replacename": "ReplaceMeCommand",
+ "commandversion": 1
},
{
- "name": "Command Group",
+ "name": "Command Group (Old)",
"description": "Create a command group",
"tags": [
"commandgroup"
],
"foldername": "commandgroup",
- "replacename": "ReplaceMeCommandGroup"
+ "replacename": "ReplaceMeCommandGroup",
+ "commandversion": 1
},
{
- "name": "Instant Command",
+ "name": "Instant Command (Old)",
"description": "A command that runs immediately",
"tags": [
"instantcommand"
],
"foldername": "instant",
- "replacename": "ReplaceMeInstantCommand"
+ "replacename": "ReplaceMeInstantCommand",
+ "commandversion": 1
},
{
- "name": "Subsystem",
+ "name": "Subsystem (Old)",
"description": "A subsystem",
"tags": [
"subsystem"
],
"foldername": "subsystem",
- "replacename": "ReplaceMeSubsystem"
+ "replacename": "ReplaceMeSubsystem",
+ "commandversion": 1
},
{
- "name": "PID Subsystem",
+ "name": "PID Subsystem (Old)",
"description": "A subsystem that runs a PID loop",
"tags": [
"pidsubsystem",
"pid"
],
"foldername": "pidsubsystem",
- "replacename": "ReplaceMePIDSubsystem"
+ "replacename": "ReplaceMePIDSubsystem",
+ "commandversion": 1
},
{
- "name": "Timed Command",
+ "name": "Timed Command (Old)",
"description": "A command that runs for a specified time",
"tags": [
"timedcommand"
],
"foldername": "timed",
- "replacename": "ReplaceMeTimedCommand"
+ "replacename": "ReplaceMeTimedCommand",
+ "commandversion": 1
},
{
- "name": "Trigger",
+ "name": "Trigger (Old)",
"description": "A command that runs off of a trigger",
"tags": [
"trigger"
],
"foldername": "trigger",
- "replacename": "ReplaceMeTrigger"
+ "replacename": "ReplaceMeTrigger",
+ "commandversion": 1
+ },
+ {
+ "name": "Command (New)",
+ "description": "A command.",
+ "tags": [
+ "command"
+ ],
+ "foldername": "command2",
+ "replacename": "ReplaceMeCommand",
+ "commandversion": 2
+ },
+ {
+ "name": "InstantCommand (New)",
+ "description": "A command that finishes instantly.",
+ "tags": [
+ "instantcommand"
+ ],
+ "foldername": "instantcommand",
+ "replacename": "ReplaceMeInstantCommand",
+ "commandversion": 2
+ },
+ {
+ "name": "ParallelCommandGroup (New)",
+ "description": "A command group that runs commands in parallel, ending when all commands have finished.",
+ "tags": [
+ "parallelcommandgroup"
+ ],
+ "foldername": "parallelcommandgroup",
+ "replacename": "ReplaceMeParallelCommandGroup",
+ "commandversion": 2
+ },
+ {
+ "name": "ParallelDeadlineGroup (New)",
+ "description": "A command group that runs commands in parallel, ending when a specific command has finished.",
+ "tags": [
+ "paralleldeadlinegroup"
+ ],
+ "foldername": "paralleldeadlinegroup",
+ "replacename": "ReplaceMeParallelDeadlineGroup",
+ "commandversion": 2
+ },
+ {
+ "name": "ParallelRaceGroup (New)",
+ "description": "A command that runs commands in parallel, ending as soon as any command has finished.",
+ "tags": [
+ "parallelracegroup"
+ ],
+ "foldername": "parallelracegroup",
+ "replacename": "ReplaceMeParallelRaceGroup",
+ "commandversion": 2
+ },
+ {
+ "name": "PIDCommand (New)",
+ "description": "A command that runs a PIDController.",
+ "tags": [
+ "pidcommand"
+ ],
+ "foldername": "pidcommand",
+ "replacename": "ReplaceMePIDCommand",
+ "commandversion": 2
+ },
+ {
+ "name": "PIDSubsystem (New)",
+ "description": "A subsystem that runs a PIDController.",
+ "tags": [
+ "pidsubsystem"
+ ],
+ "foldername": "pidsubsystem2",
+ "replacename": "ReplaceMePIDSubsystem",
+ "commandversion": 2
+ },
+ {
+ "name": "ProfiledPIDCommand (New)",
+ "description": "A command that runs a ProfiledPIDController.",
+ "tags": [
+ "profiledpidcommand"
+ ],
+ "foldername": "profiledpidcommand",
+ "replacename": "ReplaceMeProfiledPIDCommand",
+ "commandversion": 2
+ },
+ {
+ "name": "ProfiledPIDSubsystem (New)",
+ "description": "A subsystem that runs a ProfiledPIDController.",
+ "tags": [
+ "profiledpidsubsystem"
+ ],
+ "foldername": "profiledpidsubsystem",
+ "replacename": "ReplaceMeProfiledPIDSubsystem",
+ "commandversion": 2
+ },
+ {
+ "name": "SequentialCommandGroup (New)",
+ "description": "A command group that runs commands in sequence.",
+ "tags": [
+ "sequentialcommandgroup"
+ ],
+ "foldername": "sequentialcommandgroup",
+ "replacename": "ReplaceMeSequentialCommandGroup",
+ "commandversion": 2
+ },
+ {
+ "name": "Subsystem (New)",
+ "description": "A robot subsystem.",
+ "tags": [
+ "subsystem"
+ ],
+ "foldername": "subsystem2",
+ "replacename": "ReplaceMeSubsystem",
+ "commandversion": 2
+ },
+ {
+ "name": "TrapezoidProfileCommand (New)",
+ "description": "A command that executes a trapezoidal motion profile.",
+ "tags": [
+ "trapezoidprofilecommand"
+ ],
+ "foldername": "trapezoidprofilecommand",
+ "replacename": "ReplaceMeTrapezoidProfileCommand",
+ "commandversion": 2
+ },
+ {
+ "name": "TrapezoidProfileSubsystem (New)",
+ "description": "A command that executes a trapezoidal motion profile.",
+ "tags": [
+ "trapezoidprofilesubsystem"
+ ],
+ "foldername": "trapezoidprofilesubsystem",
+ "replacename": "ReplaceMeTrapezoidProfileSubsystem",
+ "commandversion": 2
}
]
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instantcommand/ReplaceMeInstantCommand.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instantcommand/ReplaceMeInstantCommand.java
new file mode 100644
index 0000000..6764ed7
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instantcommand/ReplaceMeInstantCommand.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.commands.instantcommand;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+// NOTE: Consider using this command inline, rather than writing a subclass. For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class ReplaceMeInstantCommand extends InstantCommand {
+ public ReplaceMeInstantCommand() {
+ // Use addRequirements() here to declare subsystem dependencies.
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.java
new file mode 100644
index 0000000..f293c41
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.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.wpilibj.commands.parallelcommandgroup;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+
+// NOTE: Consider using this command inline, rather than writing a subclass. For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class ReplaceMeParallelCommandGroup extends ParallelCommandGroup {
+ /**
+ * Creates a new ReplaceMeParallelCommandGroup.
+ */
+ public ReplaceMeParallelCommandGroup() {
+ // Add your commands in the super() call, e.g.
+ // super(new FooCommand(), new BarCommand());super();
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.java
new file mode 100644
index 0000000..9092834
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.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.commands.paralleldeadlinegroup;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
+
+// NOTE: Consider using this command inline, rather than writing a subclass. For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class ReplaceMeParallelDeadlineGroup extends ParallelDeadlineGroup {
+ /**
+ * Creates a new ReplaceMeParallelDeadlineGroup.
+ */
+ public ReplaceMeParallelDeadlineGroup() {
+ // Add your commands in the super() call. Add the deadline first.
+ super(
+ new InstantCommand()
+ );
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java
new file mode 100644
index 0000000..686f339
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.commands.parallelracegroup;
+
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
+
+// NOTE: Consider using this command inline, rather than writing a subclass. For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class ReplaceMeParallelRaceGroup extends ParallelRaceGroup {
+ /**
+ * Creates a new ReplaceMeParallelRaceGroup.
+ */
+ public ReplaceMeParallelRaceGroup() {
+ // Add your commands in the super() call, e.g.
+ // super(new FooCommand(), new BarCommand());
+ super();
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidcommand/ReplaceMePIDCommand.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidcommand/ReplaceMePIDCommand.java
new file mode 100644
index 0000000..2615b08
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidcommand/ReplaceMePIDCommand.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.wpilibj.commands.pidcommand;
+
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+
+// NOTE: Consider using this command inline, rather than writing a subclass. For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class ReplaceMePIDCommand extends PIDCommand {
+ /**
+ * Creates a new ReplaceMePIDCommand.
+ */
+ public ReplaceMePIDCommand() {
+ super(
+ // The controller that the command will use
+ new PIDController(0, 0, 0),
+ // This should return the measurement
+ () -> 0,
+ // This should return the setpoint (can also be a constant)
+ () -> 0,
+ // This uses the output
+ output -> {
+ // Use the output here
+ });
+ // Use addRequirements() here to declare subsystem dependencies.
+ // Configure additional PID options by calling `getController` here.
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem2/ReplaceMePIDSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem2/ReplaceMePIDSubsystem.java
new file mode 100644
index 0000000..7d5e7b3
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem2/ReplaceMePIDSubsystem.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.commands.pidsubsystem2;
+
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.PIDSubsystem;
+
+public class ReplaceMePIDSubsystem extends PIDSubsystem {
+ /**
+ * Creates a new ReplaceMePIDSubsystem.
+ */
+ public ReplaceMePIDSubsystem() {
+ super(
+ // The PIDController used by the subsystem
+ new PIDController(0, 0, 0));
+ }
+
+ @Override
+ public void useOutput(double output, double setpoint) {
+ // Use the output here
+ }
+
+ @Override
+ public double getMeasurement() {
+ // Return the process variable measurement here
+ return 0;
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.java
new file mode 100644
index 0000000..91a6e82
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.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.commands.profiledpidcommand;
+
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
+
+// NOTE: Consider using this command inline, rather than writing a subclass. For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class ReplaceMeProfiledPIDCommand extends ProfiledPIDCommand {
+ /**
+ * Creates a new ReplaceMeProfiledPIDCommand.
+ */
+ public ReplaceMeProfiledPIDCommand() {
+ super(
+ // The ProfiledPIDController used by the command
+ new ProfiledPIDController(
+ // The PID gains
+ 0, 0, 0,
+ // The motion profile constraints
+ new TrapezoidProfile.Constraints(0, 0)),
+ // This should return the measurement
+ () -> 0,
+ // This should return the goal (can also be a constant)
+ () -> new TrapezoidProfile.State(),
+ // This uses the output
+ (output, setpoint) -> {
+ // Use the output (and setpoint, if desired) here
+ });
+ // Use addRequirements() here to declare subsystem dependencies.
+ // Configure additional PID options by calling `getController` here.
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.java
new file mode 100644
index 0000000..b380d4e
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.java
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC 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.profiledpidsubsystem;
+
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
+
+public class ReplaceMeProfiledPIDSubsystem extends ProfiledPIDSubsystem {
+ /**
+ * Creates a new ReplaceMeProfiledPIDSubsystem.
+ */
+ public ReplaceMeProfiledPIDSubsystem() {
+ super(
+ // The ProfiledPIDController used by the subsystem
+ new ProfiledPIDController(0, 0, 0,
+ // The motion profile constraints
+ new TrapezoidProfile.Constraints(0, 0)));
+ }
+
+ @Override
+ public void useOutput(double output, TrapezoidProfile.State setpoint) {
+ // Use the output (and optionally the setpoint) here
+ }
+
+ @Override
+ public double getMeasurement() {
+ // Return the process variable measurement here
+ return 0;
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java
new file mode 100644
index 0000000..f08e2dd
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.commands.sequentialcommandgroup;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+// NOTE: Consider using this command inline, rather than writing a subclass. For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class ReplaceMeSequentialCommandGroup extends SequentialCommandGroup {
+ /**
+ * Creates a new ReplaceMeSequentialCommandGroup.
+ */
+ public ReplaceMeSequentialCommandGroup() {
+ // Add your commands in the super() call, e.g.
+ // super(new FooCommand(), new BarCommand());
+ super();
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem2/ReplaceMeSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem2/ReplaceMeSubsystem.java
new file mode 100644
index 0000000..0fa5838
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem2/ReplaceMeSubsystem.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.commands.subsystem2;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class ReplaceMeSubsystem extends SubsystemBase {
+ /**
+ * Creates a new ReplaceMeSubsystem.
+ */
+ public ReplaceMeSubsystem() {
+
+ }
+
+ @Override
+ public void periodic() {
+ // This method will be called once per scheduler run
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java
new file mode 100644
index 0000000..d779a22
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.commands.trapezoidprofilecommand;
+
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
+
+// NOTE: Consider using this command inline, rather than writing a subclass. For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class ReplaceMeTrapezoidProfileCommand extends TrapezoidProfileCommand {
+ /**
+ * Creates a new ReplaceMeTrapezoidProfileCommand.
+ */
+ public ReplaceMeTrapezoidProfileCommand() {
+ super(
+ // The motion profile to be executed
+ new TrapezoidProfile(
+ // The motion profile constraints
+ new TrapezoidProfile.Constraints(0, 0),
+ // Goal state
+ new TrapezoidProfile.State(),
+ // Initial state
+ new TrapezoidProfile.State()),
+ state -> {
+ // Use current trajectory state here
+ });
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.java
new file mode 100644
index 0000000..46f1b44
--- /dev/null
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.commands.trapezoidprofilesubsystem;
+
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;
+
+public class ReplaceMeTrapezoidProfileSubsystem extends TrapezoidProfileSubsystem {
+ /**
+ * Creates a new ReplaceMeTrapezoidProfileSubsystem.
+ */
+ public ReplaceMeTrapezoidProfileSubsystem() {
+ super(
+ // The constraints for the generated profiles
+ new TrapezoidProfile.Constraints(0, 0),
+ // The initial position of the mechanism
+ 0);
+ }
+
+ @Override
+ protected void useState(TrapezoidProfile.State state) {
+ // Use the computed profile state here.
+ }
+}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java
index b89365b..a26031b 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java
@@ -19,9 +19,9 @@
@Override
public void robotInit() {
- // PWM port 0
+ // PWM port 9
// Must be a PWM header, not MXP or DIO
- m_led = new AddressableLED(0);
+ m_led = new AddressableLED(9);
// Reuse buffer
// Default to a length of 60, start empty output
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
index 64f44d6..5558f25 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
@@ -14,11 +14,11 @@
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj.examples.armbot.Constants.OIConstants;
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
@@ -32,7 +32,7 @@
private final ArmSubsystem m_robotArm = new ArmSubsystem();
// The driver's controller
- XboxController m_driverController = new XboxController(kDriverControllerPort);
+ XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
index 7d549e5..d9fd206 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
@@ -14,40 +14,28 @@
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;
+import edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants;
/**
* 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 PWMVictorSPX m_motor = new PWMVictorSPX(ArmConstants.kMotorPort);
+ private final Encoder m_encoder =
+ new Encoder(ArmConstants.kEncoderPorts[0], ArmConstants.kEncoderPorts[1]);
private final ArmFeedforward m_feedforward =
- new ArmFeedforward(kSVolts, kCosVolts, kVVoltSecondPerRad, kAVoltSecondSquaredPerRad);
+ new ArmFeedforward(ArmConstants.kSVolts, ArmConstants.kCosVolts,
+ ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
/**
* Create a new ArmSubsystem.
*/
public ArmSubsystem() {
- super(new ProfiledPIDController(
- kP,
- 0,
- 0,
- new TrapezoidProfile.Constraints(kMaxVelocityRadPerSecond,
- kMaxAccelerationRadPerSecSquared)));
- m_encoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ super(new ProfiledPIDController(ArmConstants.kP, 0, 0, new TrapezoidProfile.Constraints(
+ ArmConstants.kMaxVelocityRadPerSecond, ArmConstants.kMaxAccelerationRadPerSecSquared)), 0);
+ m_encoder.setDistancePerPulse(ArmConstants.kEncoderDistancePerPulse);
// Start arm at rest in neutral position
- setGoal(kArmOffsetRads);
+ setGoal(ArmConstants.kArmOffsetRads);
}
@Override
@@ -60,6 +48,6 @@
@Override
public double getMeasurement() {
- return m_encoder.getDistance() + kArmOffsetRads;
+ return m_encoder.getDistance() + ArmConstants.kArmOffsetRads;
}
}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
index a22079e..9404013 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
@@ -13,45 +13,39 @@
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;
+import edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants;
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));
+ new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
+ new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
// The motors on the right side of the drive.
private final SpeedControllerGroup m_rightMotors =
- new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
- new PWMVictorSPX(kRightMotor2Port));
+ new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
+ new PWMVictorSPX(DriveConstants.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);
+ new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
+ DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
- new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+ new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
/**
* Creates a new DriveSubsystem.
*/
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
- m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
- m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
}
/**
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java
index 285bd58..6bca84a 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java
@@ -14,11 +14,11 @@
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.OIConstants;
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
@@ -32,7 +32,7 @@
private final ArmSubsystem m_robotArm = new ArmSubsystem();
// The driver's controller
- XboxController m_driverController = new XboxController(kDriverControllerPort);
+ XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
index 2d2698c..92fd43c 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
@@ -11,34 +11,27 @@
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;
+import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants;
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 ExampleSmartMotorController m_motor =
+ new ExampleSmartMotorController(ArmConstants.kMotorPort);
private final ArmFeedforward m_feedforward =
- new ArmFeedforward(kSVolts, kCosVolts, kVVoltSecondPerRad, kAVoltSecondSquaredPerRad);
+ new ArmFeedforward(ArmConstants.kSVolts, ArmConstants.kCosVolts,
+ ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
/**
* Create a new ArmSubsystem.
*/
public ArmSubsystem() {
- super(new TrapezoidProfile.Constraints(kMaxVelocityRadPerSecond,
- kMaxAccelerationRadPerSecSquared),
- kArmOffsetRads);
- m_motor.setPID(kP, 0, 0);
+ super(new TrapezoidProfile.Constraints(ArmConstants.kMaxVelocityRadPerSecond,
+ ArmConstants.kMaxAccelerationRadPerSecSquared),
+ ArmConstants.kArmOffsetRads);
+ m_motor.setPID(ArmConstants.kP, 0, 0);
}
@Override
@@ -46,8 +39,7 @@
// 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,
+ m_motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, setpoint.position,
feedforward / 12.0);
}
}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
index 974f514..433dba4 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
@@ -13,45 +13,39 @@
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;
+import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants;
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));
+ new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
+ new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
// The motors on the right side of the drive.
private final SpeedControllerGroup m_rightMotors =
- new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
- new PWMVictorSPX(kRightMotor2Port));
+ new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
+ new PWMVictorSPX(DriveConstants.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);
+ new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
+ DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
- new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+ new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
/**
* Creates a new DriveSubsystem.
*/
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
- m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
- m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
}
/**
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java
index 10df8b6..f7ff493 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java
@@ -11,9 +11,6 @@
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
-import static edu.wpi.first.wpilibj.examples.differentialdrivebot.Drivetrain.kMaxAngularSpeed;
-import static edu.wpi.first.wpilibj.examples.differentialdrivebot.Drivetrain.kMaxSpeed;
-
public class Robot extends TimedRobot {
private final XboxController m_controller = new XboxController(0);
private final Drivetrain m_drive = new Drivetrain();
@@ -28,13 +25,13 @@
public void teleopPeriodic() {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
- final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * kMaxSpeed;
+ final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * Drivetrain.kMaxSpeed;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
- final var rot = -m_controller.getX(GenericHID.Hand.kRight) * kMaxAngularSpeed;
+ final var rot = -m_controller.getX(GenericHID.Hand.kRight) * Drivetrain.kMaxAngularSpeed;
m_drive.drive(xSpeed, rot);
}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
index a0eb86a..f618b9d 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
@@ -16,13 +16,12 @@
import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.OIConstants;
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
@@ -35,7 +34,7 @@
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
// The driver's controller
- XboxController m_driverController = new XboxController(kDriverControllerPort);
+ XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -72,8 +71,9 @@
new TrapezoidProfileCommand(
new TrapezoidProfile(
// Limit the max acceleration and velocity
- new TrapezoidProfile.Constraints(kMaxSpeedMetersPerSecond,
- kMaxAccelerationMetersPerSecondSquared),
+ new TrapezoidProfile.Constraints(
+ DriveConstants.kMaxSpeedMetersPerSecond,
+ DriveConstants.kMaxAccelerationMetersPerSecondSquared),
// End at desired position in meters; implicitly starts at 0
new TrapezoidProfile.State(3, 0)),
// Pipe the profile state to the drive
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
index f89b36a..280acd3 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
@@ -10,11 +10,9 @@
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
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.
*/
@@ -29,8 +27,8 @@
super(
new TrapezoidProfile(
// Limit the max acceleration and velocity
- new TrapezoidProfile.Constraints(kMaxSpeedMetersPerSecond,
- kMaxAccelerationMetersPerSecondSquared),
+ new TrapezoidProfile.Constraints(DriveConstants.kMaxSpeedMetersPerSecond,
+ DriveConstants.kMaxAccelerationMetersPerSecondSquared),
// End at desired position in meters; implicitly starts at 0
new TrapezoidProfile.State(meters, 0)),
// Pipe the profile state to the drive
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
index c8b98f7..ac55100 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
@@ -12,35 +12,27 @@
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
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);
+ new ExampleSmartMotorController(DriveConstants.kLeftMotor1Port);
private final ExampleSmartMotorController m_leftSlave =
- new ExampleSmartMotorController(kLeftMotor2Port);
+ new ExampleSmartMotorController(DriveConstants.kLeftMotor2Port);
private final ExampleSmartMotorController m_rightMaster =
- new ExampleSmartMotorController(kRightMotor1Port);
+ new ExampleSmartMotorController(DriveConstants.kRightMotor1Port);
private final ExampleSmartMotorController m_rightSlave =
- new ExampleSmartMotorController(kRightMotor2Port);
+ new ExampleSmartMotorController(DriveConstants.kRightMotor2Port);
private final SimpleMotorFeedforward m_feedforward =
- new SimpleMotorFeedforward(ksVolts,
- kvVoltSecondsPerMeter,
- kaVoltSecondsSquaredPerMeter);
+ new SimpleMotorFeedforward(DriveConstants.ksVolts,
+ DriveConstants.kvVoltSecondsPerMeter,
+ DriveConstants.kaVoltSecondsSquaredPerMeter);
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMaster, m_rightMaster);
@@ -52,8 +44,8 @@
m_leftSlave.follow(m_leftMaster);
m_rightSlave.follow(m_rightMaster);
- m_leftMaster.setPID(kp, 0, 0);
- m_rightMaster.setPID(kp, 0, 0);
+ m_leftMaster.setPID(DriveConstants.kp, 0, 0);
+ m_rightMaster.setPID(DriveConstants.kp, 0, 0);
}
/**
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
index 69008ae..957f11f 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
@@ -17,13 +17,12 @@
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants;
+import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.ShooterSubsystem;
import static edu.wpi.first.wpilibj.XboxController.Button;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants.kAutoShootTimeSeconds;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants.kAutoTimeoutSeconds;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.OIConstants.kDriverControllerPort;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -45,10 +44,10 @@
// Start running the feeder
new InstantCommand(m_shooter::runFeeder, m_shooter),
// Shoot for the specified time
- new WaitCommand(kAutoShootTimeSeconds))
+ new WaitCommand(AutoConstants.kAutoShootTimeSeconds))
// Add a timeout (will end the command if, for instance, the shooter never gets up to
// speed)
- .withTimeout(kAutoTimeoutSeconds)
+ .withTimeout(AutoConstants.kAutoTimeoutSeconds)
// When the command ends, turn off the shooter and the feeder
.andThen(() -> {
m_shooter.disable();
@@ -56,7 +55,7 @@
});
// The driver's controller
- XboxController m_driverController = new XboxController(kDriverControllerPort);
+ XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -72,7 +71,7 @@
// 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));
+ m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
index 192fa30..b82cb75 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
@@ -13,45 +13,39 @@
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kEncoderDistancePerPulse;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftEncoderPorts;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftEncoderReversed;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftMotor1Port;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftMotor2Port;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightEncoderPorts;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightEncoderReversed;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightMotor1Port;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightMotor2Port;
+import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants;
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));
+ new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
+ new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
// The motors on the right side of the drive.
private final SpeedControllerGroup m_rightMotors =
- new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
- new PWMVictorSPX(kRightMotor2Port));
+ new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
+ new PWMVictorSPX(DriveConstants.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);
+ new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
+ DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
- new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+ new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
/**
* Creates a new DriveSubsystem.
*/
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
- m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
- m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
}
/**
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
index c870114..8b0f3ca 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
@@ -13,36 +13,26 @@
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;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderDistancePerPulse;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderPorts;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderReversed;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kFeederMotorPort;
-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.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.kVVoltSecondsPerRotation;
+import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants;
public class ShooterSubsystem extends PIDSubsystem {
- private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(kShooterMotorPort);
- private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(kFeederMotorPort);
+ private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(ShooterConstants.kShooterMotorPort);
+ private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(ShooterConstants.kFeederMotorPort);
private final Encoder m_shooterEncoder =
- new Encoder(kEncoderPorts[0], kEncoderPorts[1], kEncoderReversed);
- private final SimpleMotorFeedforward m_shooterFeedforward
- = new SimpleMotorFeedforward(kSVolts, kVVoltSecondsPerRotation);
+ new Encoder(ShooterConstants.kEncoderPorts[0], ShooterConstants.kEncoderPorts[1],
+ ShooterConstants.kEncoderReversed);
+ private final SimpleMotorFeedforward m_shooterFeedforward =
+ new SimpleMotorFeedforward(ShooterConstants.kSVolts,
+ ShooterConstants.kVVoltSecondsPerRotation);
/**
* The shooter subsystem for the robot.
*/
public ShooterSubsystem() {
- super(new PIDController(kP, kI, kD));
- getController().setTolerance(kShooterToleranceRPS);
- m_shooterEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
- setSetpoint(kShooterTargetRPS);
+ super(new PIDController(ShooterConstants.kP, ShooterConstants.kI, ShooterConstants.kD));
+ getController().setTolerance(ShooterConstants.kShooterToleranceRPS);
+ m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
+ setSetpoint(ShooterConstants.kShooterTargetRPS);
}
@Override
@@ -60,7 +50,7 @@
}
public void runFeeder() {
- m_feederMotor.set(kFeederSpeed);
+ m_feederMotor.set(ShooterConstants.kFeederSpeed);
}
public void stopFeeder() {
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
index 44b09b8..1b94fea 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
@@ -16,15 +16,13 @@
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.OIConstants;
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;
-import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationD;
-import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationI;
-import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationP;
-import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.OIConstants.kDriverControllerPort;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -37,7 +35,7 @@
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
// The driver's controller
- XboxController m_driverController = new XboxController(kDriverControllerPort);
+ XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -53,7 +51,7 @@
// 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));
+ m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
}
@@ -70,18 +68,17 @@
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
// Stabilize robot to drive straight with gyro when left bumper is held
- new JoystickButton(m_driverController, Button.kBumperLeft.value).whenHeld(
- new PIDCommand(
- new PIDController(kStabilizationP, kStabilizationI, kStabilizationD),
- // Close the loop on the turn rate
- m_robotDrive::getTurnRate,
- // Setpoint is 0
- 0,
- // Pipe the output to the turning controls
- output -> m_robotDrive
- .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), output),
- // Require the robot drive
- m_robotDrive));
+ new JoystickButton(m_driverController, Button.kBumperLeft.value).whenHeld(new PIDCommand(
+ new PIDController(DriveConstants.kStabilizationP, DriveConstants.kStabilizationI,
+ DriveConstants.kStabilizationD),
+ // Close the loop on the turn rate
+ m_robotDrive::getTurnRate,
+ // Setpoint is 0
+ 0,
+ // Pipe the output to the turning controls
+ output -> m_robotDrive.arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), output),
+ // Require the robot drive
+ m_robotDrive));
// Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout
new JoystickButton(m_driverController, Button.kX.value)
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java
index fe6c725..6c8ea36 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java
@@ -10,14 +10,9 @@
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj2.command.PIDCommand;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
-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.
*/
@@ -29,7 +24,8 @@
* @param drive The drive subsystem to use
*/
public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) {
- super(new PIDController(kTurnP, kTurnI, kTurnD),
+ super(
+ new PIDController(DriveConstants.kTurnP, DriveConstants.kTurnI, DriveConstants.kTurnD),
// Close loop on heading
drive::getHeading,
// Set reference to target
@@ -43,7 +39,8 @@
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);
+ getController()
+ .setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
}
@Override
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java
index 8fb4320..44f24d3 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java
@@ -11,16 +11,9 @@
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
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.
*/
@@ -33,11 +26,10 @@
*/
public TurnToAngleProfiled(double targetAngleDegrees, DriveSubsystem drive) {
super(
- new ProfiledPIDController(kTurnP, kTurnI, kTurnD,
- new TrapezoidProfile.Constraints(
- kMaxTurnRateDegPerS,
- kMaxTurnAccelerationDegPerSSquared
- )),
+ new ProfiledPIDController(DriveConstants.kTurnP, DriveConstants.kTurnI,
+ DriveConstants.kTurnD, new TrapezoidProfile.Constraints(
+ DriveConstants.kMaxTurnRateDegPerS,
+ DriveConstants.kMaxTurnAccelerationDegPerSSquared)),
// Close loop on heading
drive::getHeading,
// Set reference to target
@@ -51,7 +43,8 @@
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);
+ getController()
+ .setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
}
@Override
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
index a4f001e..7bdeb24 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
@@ -15,38 +15,31 @@
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kEncoderDistancePerPulse;
-import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kGyroReversed;
-import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftEncoderPorts;
-import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftEncoderReversed;
-import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftMotor1Port;
-import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftMotor2Port;
-import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightEncoderPorts;
-import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightEncoderReversed;
-import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightMotor1Port;
-import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightMotor2Port;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
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));
+ new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
+ new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
// The motors on the right side of the drive.
private final SpeedControllerGroup m_rightMotors =
- new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
- new PWMVictorSPX(kRightMotor2Port));
+ new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
+ new PWMVictorSPX(DriveConstants.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);
+ new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
+ DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
- new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+ new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
// The gyro sensor
private final Gyro m_gyro = new ADXRS450_Gyro();
@@ -56,8 +49,8 @@
*/
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
- m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
- m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
}
/**
@@ -127,7 +120,7 @@
* @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);
+ return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
}
/**
@@ -136,6 +129,6 @@
* @return The turn rate of the robot, in degrees per second
*/
public double getTurnRate() {
- return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0);
+ return m_gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
}
}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
index a5c7129..2250123 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
@@ -17,14 +17,13 @@
import edu.wpi.first.wpilibj2.command.StartEndCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.commands.ComplexAutoCommand;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem;
import static edu.wpi.first.wpilibj.XboxController.Button;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveDistanceInches;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveSpeed;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.OIConstants.kDriverControllerPort;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -40,20 +39,18 @@
// The autonomous routines
// A simple auto routine that drives forward a specified distance, and then stops.
- private final Command m_simpleAuto =
- new StartEndCommand(
- // Start driving forward at the start of the command
- () -> m_robotDrive.arcadeDrive(kAutoDriveSpeed, 0),
- // Stop driving at the end of the command
- () -> m_robotDrive.arcadeDrive(0, 0),
- // Requires the drive subsystem
- m_robotDrive
- )
- // Reset the encoders before starting
- .beforeStarting(m_robotDrive::resetEncoders)
- // End the command when the robot's driven distance exceeds the desired value
- .withInterrupt(() -> m_robotDrive.getAverageEncoderDistance()
- >= kAutoDriveDistanceInches);
+ private final Command m_simpleAuto = new StartEndCommand(
+ // Start driving forward at the start of the command
+ () -> m_robotDrive.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0),
+ // Stop driving at the end of the command
+ () -> m_robotDrive.arcadeDrive(0, 0),
+ // Requires the drive subsystem
+ m_robotDrive)
+ // Reset the encoders before starting
+ .beforeStarting(m_robotDrive::resetEncoders)
+ // End the command when the robot's driven distance exceeds the desired value
+ .withInterrupt(
+ () -> m_robotDrive.getAverageEncoderDistance() >= AutoConstants.kAutoDriveDistanceInches);
// A complex auto routine that drives forward, drops a hatch, and then drives backward.
private final Command m_complexAuto = new ComplexAutoCommand(m_robotDrive, m_hatchSubsystem);
@@ -62,7 +59,7 @@
SendableChooser<Command> m_chooser = new SendableChooser<>();
// The driver's controller
- XboxController m_driverController = new XboxController(kDriverControllerPort);
+ XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -76,11 +73,9 @@
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)
- );
+ new RunCommand(() -> m_robotDrive
+ .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
+ m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
// Add commands to the autonomous command chooser
m_chooser.addOption("Simple Auto", m_simpleAuto);
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java
index f46bcf2..45674da 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java
@@ -11,13 +11,10 @@
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.StartEndCommand;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoBackupDistanceInches;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveDistanceInches;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveSpeed;
-
/**
* A complex auto command that drives forward, releases a hatch, and then drives backward.
*/
@@ -33,29 +30,26 @@
// Drive forward up to the front of the cargo ship
new StartEndCommand(
// Start driving forward at the start of the command
- () -> driveSubsystem.arcadeDrive(kAutoDriveSpeed, 0),
+ () -> driveSubsystem.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0),
// Stop driving at the end of the command
- () -> driveSubsystem.arcadeDrive(0, 0), driveSubsystem
- )
+ () -> driveSubsystem.arcadeDrive(0, 0), driveSubsystem)
// Reset the encoders before starting
.beforeStarting(driveSubsystem::resetEncoders)
// End the command when the robot's driven distance exceeds the desired value
- .withInterrupt(
- () -> driveSubsystem.getAverageEncoderDistance() >= kAutoDriveDistanceInches),
+ .withInterrupt(() -> driveSubsystem.getAverageEncoderDistance()
+ >= AutoConstants.kAutoDriveDistanceInches),
// Release the hatch
new InstantCommand(hatchSubsystem::releaseHatch, hatchSubsystem),
// Drive backward the specified distance
new StartEndCommand(
- () -> driveSubsystem.arcadeDrive(-kAutoDriveSpeed, 0),
- () -> driveSubsystem.arcadeDrive(0, 0),
- driveSubsystem
- )
+ () -> driveSubsystem.arcadeDrive(-AutoConstants.kAutoDriveSpeed, 0),
+ () -> driveSubsystem.arcadeDrive(0, 0), driveSubsystem)
.beforeStarting(driveSubsystem::resetEncoders)
.withInterrupt(
- () -> driveSubsystem.getAverageEncoderDistance() <= -kAutoBackupDistanceInches)
- );
+ () -> driveSubsystem.getAverageEncoderDistance()
+ <= -AutoConstants.kAutoBackupDistanceInches));
}
}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
index 5361e87..5d16a44 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
@@ -13,45 +13,39 @@
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kEncoderDistancePerPulse;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftEncoderPorts;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftEncoderReversed;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftMotor1Port;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftMotor2Port;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightEncoderPorts;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightEncoderReversed;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightMotor1Port;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightMotor2Port;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants;
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));
+ new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
+ new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
// The motors on the right side of the drive.
private final SpeedControllerGroup m_rightMotors =
- new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
- new PWMVictorSPX(kRightMotor2Port));
+ new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
+ new PWMVictorSPX(DriveConstants.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);
+ new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
+ DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
- new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+ new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
/**
* Creates a new DriveSubsystem.
*/
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
- m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
- m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
}
/**
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java
index 6dce9e1..92e4979 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java
@@ -10,17 +10,18 @@
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants;
+
import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants.kHatchSolenoidModule;
-import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants.kHatchSolenoidPorts;
/**
* A hatch mechanism actuated by a single {@link edu.wpi.first.wpilibj.DoubleSolenoid}.
*/
public class HatchSubsystem extends SubsystemBase {
private final DoubleSolenoid m_hatchSolenoid =
- new DoubleSolenoid(kHatchSolenoidModule, kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]);
+ new DoubleSolenoid(HatchConstants.kHatchSolenoidModule, HatchConstants.kHatchSolenoidPorts[0],
+ HatchConstants.kHatchSolenoidPorts[1]);
/**
* Grabs the hatch.
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java
index f839768..3e2dc82 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java
@@ -14,6 +14,8 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.ComplexAuto;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.DefaultDrive;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.DriveDistance;
@@ -24,9 +26,6 @@
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
import static edu.wpi.first.wpilibj.XboxController.Button;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveDistanceInches;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveSpeed;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.OIConstants.kDriverControllerPort;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -43,7 +42,8 @@
// A simple auto routine that drives forward a specified distance, and then stops.
private final Command m_simpleAuto =
- new DriveDistance(kAutoDriveDistanceInches, kAutoDriveSpeed, m_robotDrive);
+ new DriveDistance(AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed,
+ m_robotDrive);
// A complex auto routine that drives forward, drops a hatch, and then drives backward.
private final Command m_complexAuto = new ComplexAuto(m_robotDrive, m_hatchSubsystem);
@@ -52,7 +52,7 @@
SendableChooser<Command> m_chooser = new SendableChooser<>();
// The driver's controller
- XboxController m_driverController = new XboxController(kDriverControllerPort);
+ XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -69,8 +69,7 @@
new DefaultDrive(
m_robotDrive,
() -> m_driverController.getY(GenericHID.Hand.kLeft),
- () -> m_driverController.getX(GenericHID.Hand.kRight))
- );
+ () -> m_driverController.getX(GenericHID.Hand.kRight)));
// Add commands to the autonomous command chooser
m_chooser.addOption("Simple Auto", m_simpleAuto);
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java
index 9614922..5f776a5 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java
@@ -9,13 +9,10 @@
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoBackupDistanceInches;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveDistanceInches;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveSpeed;
-
/**
* A complex auto command that drives forward, releases a hatch, and then drives backward.
*/
@@ -29,14 +26,15 @@
public ComplexAuto(DriveSubsystem drive, HatchSubsystem hatch) {
addCommands(
// Drive forward the specified distance
- new DriveDistance(kAutoDriveDistanceInches, kAutoDriveSpeed, drive),
+ new DriveDistance(AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed,
+ drive),
// Release the hatch
new ReleaseHatch(hatch),
// Drive backward the specified distance
- new DriveDistance(kAutoBackupDistanceInches, -kAutoDriveSpeed, drive)
- );
+ new DriveDistance(AutoConstants.kAutoBackupDistanceInches, -AutoConstants.kAutoDriveSpeed,
+ drive));
}
}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
index a068b56..f49abb9 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
@@ -13,45 +13,39 @@
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kEncoderDistancePerPulse;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftEncoderPorts;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftEncoderReversed;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftMotor1Port;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftMotor2Port;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightEncoderPorts;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightEncoderReversed;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightMotor1Port;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightMotor2Port;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants;
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));
+ new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
+ new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
// The motors on the right side of the drive.
private final SpeedControllerGroup m_rightMotors =
- new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
- new PWMVictorSPX(kRightMotor2Port));
+ new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
+ new PWMVictorSPX(DriveConstants.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);
+ new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
+ DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
- new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+ new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
/**
* Creates a new DriveSubsystem.
*/
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
- m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
- m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
}
/**
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java
index e93fea4..47da829 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java
@@ -10,17 +10,18 @@
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants;
+
import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants.kHatchSolenoidModule;
-import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants.kHatchSolenoidPorts;
/**
* A hatch mechanism actuated by a single {@link DoubleSolenoid}.
*/
public class HatchSubsystem extends SubsystemBase {
private final DoubleSolenoid m_hatchSolenoid =
- new DoubleSolenoid(kHatchSolenoidModule, kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]);
+ new DoubleSolenoid(HatchConstants.kHatchSolenoidModule, HatchConstants.kHatchSolenoidPorts[0],
+ HatchConstants.kHatchSolenoidPorts[1]);
/**
* Grabs the hatch.
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
index b7540d3..aeb4681 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
@@ -11,9 +11,6 @@
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
-import static edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxAngularSpeed;
-import static edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed;
-
public class Robot extends TimedRobot {
private final XboxController m_controller = new XboxController(0);
private final Drivetrain m_mecanum = new Drivetrain();
@@ -32,18 +29,18 @@
private void driveWithJoystick(boolean fieldRelative) {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
- final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * kMaxSpeed;
+ final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * Drivetrain.kMaxSpeed;
// Get the y speed or sideways/strafe speed. We are inverting this because
// we want a positive value when we pull to the left. Xbox controllers
// return positive values when you pull to the right by default.
- final var ySpeed = -m_controller.getX(GenericHID.Hand.kLeft) * kMaxSpeed;
+ final var ySpeed = -m_controller.getX(GenericHID.Hand.kLeft) * Drivetrain.kMaxSpeed;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
- final var rot = -m_controller.getX(GenericHID.Hand.kRight) * kMaxAngularSpeed;
+ final var rot = -m_controller.getX(GenericHID.Hand.kRight) * Drivetrain.kMaxAngularSpeed;
m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative);
}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
index 54707e3..1ec1fc9 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
@@ -25,22 +25,11 @@
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants;
+import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.OIConstants;
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}
@@ -53,7 +42,7 @@
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
// The driver's controller
- XboxController m_driverController = new XboxController(kDriverControllerPort);
+ XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -96,9 +85,10 @@
public Command getAutonomousCommand() {
// Create config for trajectory
TrajectoryConfig config =
- new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared)
+ new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
+ AutoConstants.kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
- .setKinematics(kDriveKinematics);
+ .setKinematics(DriveConstants.kDriveKinematics);
// An example trajectory to follow. All units in meters.
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
@@ -107,7 +97,7 @@
// Pass through these two interior waypoints, making an 's' curve path
List.of(
new Translation2d(1, 1),
- new Translation2d(2, - 1)
+ new Translation2d(2, -1)
),
// End 3 meters straight ahead of where we started, facing forward
new Pose2d(3, 0, new Rotation2d(0)),
@@ -118,22 +108,23 @@
exampleTrajectory,
m_robotDrive::getPose,
- kFeedforward,
- kDriveKinematics,
+ DriveConstants.kFeedforward,
+ DriveConstants.kDriveKinematics,
//Position contollers
- new PIDController(kPXController, 0, 0),
- new PIDController(kPYController, 0, 0),
- new ProfiledPIDController(kPThetaController, 0, 0, kThetaControllerConstraints),
+ new PIDController(AutoConstants.kPXController, 0, 0),
+ new PIDController(AutoConstants.kPYController, 0, 0),
+ new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0,
+ AutoConstants.kThetaControllerConstraints),
//Needed for normalizing wheel speeds
- kMaxSpeedMetersPerSecond,
+ AutoConstants.kMaxSpeedMetersPerSecond,
//Velocity PID's
- new PIDController(kPFrontLeftVel, 0, 0),
- new PIDController(kPRearLeftVel, 0, 0),
- new PIDController(kPFrontRightVel, 0, 0),
- new PIDController(kPRearRightVel, 0, 0),
+ new PIDController(DriveConstants.kPFrontLeftVel, 0, 0),
+ new PIDController(DriveConstants.kPRearLeftVel, 0, 0),
+ new PIDController(DriveConstants.kPFrontRightVel, 0, 0),
+ new PIDController(DriveConstants.kPRearRightVel, 0, 0),
m_robotDrive::getCurrentWheelSpeeds,
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
index b68f770..7aaf114 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
@@ -19,70 +19,60 @@
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;
+import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants;
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 PWMVictorSPX m_frontLeft = new PWMVictorSPX(DriveConstants.kFrontLeftMotorPort);
+ private final PWMVictorSPX m_rearLeft = new PWMVictorSPX(DriveConstants.kRearLeftMotorPort);
+ private final PWMVictorSPX m_frontRight = new PWMVictorSPX(DriveConstants.kFrontRightMotorPort);
+ private final PWMVictorSPX m_rearRight = new PWMVictorSPX(DriveConstants.kRearRightMotorPort);
private final MecanumDrive m_drive = new MecanumDrive(
- m_frontLeft,
- m_rearLeft,
- m_frontRight,
- m_rearRight);
+ 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);
+ new Encoder(DriveConstants.kFrontLeftEncoderPorts[0],
+ DriveConstants.kFrontLeftEncoderPorts[1],
+ DriveConstants.kFrontLeftEncoderReversed);
// The rear-left-side drive encoder
private final Encoder m_rearLeftEncoder =
- new Encoder(kRearLeftEncoderPorts[0], kRearLeftEncoderPorts[1],
- kRearLeftEncoderReversed);
+ new Encoder(DriveConstants.kRearLeftEncoderPorts[0],
+ DriveConstants.kRearLeftEncoderPorts[1],
+ DriveConstants.kRearLeftEncoderReversed);
// The front-right--side drive encoder
private final Encoder m_frontRightEncoder =
- new Encoder(kFrontRightEncoderPorts[0], kFrontRightEncoderPorts[1],
- kFrontRightEncoderReversed);
+ new Encoder(DriveConstants.kFrontRightEncoderPorts[0],
+ DriveConstants.kFrontRightEncoderPorts[1],
+ DriveConstants.kFrontRightEncoderReversed);
// The rear-right-side drive encoder
private final Encoder m_rearRightEncoder =
- new Encoder(kRearRightEncoderPorts[0], kRearRightEncoderPorts[1],
- kRearRightEncoderReversed);
+ new Encoder(DriveConstants.kRearRightEncoderPorts[0],
+ DriveConstants.kRearRightEncoderPorts[1],
+ DriveConstants.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());
+ new MecanumDriveOdometry(DriveConstants.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);
+ m_frontLeftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ m_rearLeftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ m_frontRightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ m_rearRightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
}
/**
@@ -92,18 +82,18 @@
*/
public Rotation2d getAngle() {
// Negating the angle because WPILib gyros are CW positive.
- return Rotation2d.fromDegrees(m_gyro.getAngle() * (kGyroReversed ? 1.0 : -1.0));
+ return Rotation2d.fromDegrees(m_gyro.getAngle() * (DriveConstants.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()));
+ new MecanumDriveWheelSpeeds(
+ m_frontLeftEncoder.getRate(),
+ m_rearLeftEncoder.getRate(),
+ m_frontRightEncoder.getRate(),
+ m_rearRightEncoder.getRate()));
}
/**
@@ -135,7 +125,7 @@
*/
@SuppressWarnings("ParameterName")
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
- if ( fieldRelative ) {
+ if (fieldRelative) {
m_drive.driveCartesian(ySpeed, xSpeed, rot, -m_gyro.getAngle());
} else {
m_drive.driveCartesian(ySpeed, xSpeed, rot);
@@ -144,8 +134,8 @@
}
/**
- * Sets the front left drive SpeedController to a voltage.
- */
+ * Sets the front left drive SpeedController to a voltage.
+ */
public void setDriveSpeedControllersVolts(MecanumDriveMotorVoltages volts) {
m_frontLeft.setVoltage(volts.frontLeftVoltage);
m_rearLeft.setVoltage(volts.rearLeftVoltage);
@@ -193,6 +183,7 @@
public Encoder getFrontRightEncoder() {
return m_frontRightEncoder;
}
+
/**
* Gets the rear right drive encoder.
*
@@ -211,9 +202,9 @@
public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
return new MecanumDriveWheelSpeeds(m_frontLeftEncoder.getRate(),
- m_rearLeftEncoder.getRate(),
- m_frontRightEncoder.getRate(),
- m_rearRightEncoder.getRate());
+ m_rearLeftEncoder.getRate(),
+ m_frontRightEncoder.getRate(),
+ m_rearRightEncoder.getRate());
}
@@ -239,7 +230,7 @@
* @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);
+ return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
}
/**
@@ -248,6 +239,6 @@
* @return The turn rate of the robot, in degrees per second
*/
public double getTurnRate() {
- return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0);
+ return m_gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
}
}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
index 7d47655..f70a003 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
@@ -29,7 +29,7 @@
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
- public static final double kTrackwidthMeters = 0.6;
+ public static final double kTrackwidthMeters = 0.69;
public static final DifferentialDriveKinematics kDriveKinematics =
new DifferentialDriveKinematics(kTrackwidthMeters);
@@ -44,14 +44,14 @@
// 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
+ // The Robot 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 ksVolts = 0.22;
+ public static final double kvVoltSecondsPerMeter = 1.98;
+ public static final double kaVoltSecondsSquaredPerMeter = 0.2;
// Example value only - as above, this must be tuned for your drive!
- public static final double kPDriveVel = 0.5;
+ public static final double kPDriveVel = 8.5;
}
public static final class OIConstants {
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
index c575e29..8bb4e1d 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
@@ -26,19 +26,12 @@
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants;
+import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems.DriveSubsystem;
import static edu.wpi.first.wpilibj.XboxController.Button;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kRamseteB;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kRamseteZeta;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kDriveKinematics;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kPDriveVel;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kaVoltSecondsSquaredPerMeter;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.ksVolts;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kvVoltSecondsPerMeter;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.OIConstants.kDriverControllerPort;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -51,7 +44,7 @@
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
// The driver's controller
- XboxController m_driverController = new XboxController(kDriverControllerPort);
+ XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -96,17 +89,18 @@
// Create a voltage constraint to ensure we don't accelerate too fast
var autoVoltageConstraint =
new DifferentialDriveVoltageConstraint(
- new SimpleMotorFeedforward(ksVolts,
- kvVoltSecondsPerMeter,
- kaVoltSecondsSquaredPerMeter),
- kDriveKinematics,
+ new SimpleMotorFeedforward(DriveConstants.ksVolts,
+ DriveConstants.kvVoltSecondsPerMeter,
+ DriveConstants.kaVoltSecondsSquaredPerMeter),
+ DriveConstants.kDriveKinematics,
10);
// Create config for trajectory
TrajectoryConfig config =
- new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared)
+ new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
+ AutoConstants.kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
- .setKinematics(kDriveKinematics)
+ .setKinematics(DriveConstants.kDriveKinematics)
// Apply the voltage constraint
.addConstraint(autoVoltageConstraint);
@@ -128,12 +122,14 @@
RamseteCommand ramseteCommand = new RamseteCommand(
exampleTrajectory,
m_robotDrive::getPose,
- new RamseteController(kRamseteB, kRamseteZeta),
- new SimpleMotorFeedforward(ksVolts, kvVoltSecondsPerMeter, kaVoltSecondsSquaredPerMeter),
- kDriveKinematics,
+ new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
+ new SimpleMotorFeedforward(DriveConstants.ksVolts,
+ DriveConstants.kvVoltSecondsPerMeter,
+ DriveConstants.kaVoltSecondsSquaredPerMeter),
+ DriveConstants.kDriveKinematics,
m_robotDrive::getWheelSpeeds,
- new PIDController(kPDriveVel, 0, 0),
- new PIDController(kPDriveVel, 0, 0),
+ new PIDController(DriveConstants.kPDriveVel, 0, 0),
+ new PIDController(DriveConstants.kPDriveVel, 0, 0),
// RamseteCommand passes volts to the callback
m_robotDrive::tankDriveVolts,
m_robotDrive
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
index ecb640c..7b5c5e5 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
@@ -19,38 +19,31 @@
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-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;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftEncoderReversed;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftMotor1Port;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftMotor2Port;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightEncoderPorts;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightEncoderReversed;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightMotor1Port;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightMotor2Port;
+import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;
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));
+ new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
+ new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
// The motors on the right side of the drive.
private final SpeedControllerGroup m_rightMotors =
- new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
- new PWMVictorSPX(kRightMotor2Port));
+ new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
+ new PWMVictorSPX(DriveConstants.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);
+ new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
+ DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
- new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+ new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
// The gyro sensor
private final Gyro m_gyro = new ADXRS450_Gyro();
@@ -63,8 +56,8 @@
*/
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
- m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
- m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
resetEncoders();
m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));
@@ -74,7 +67,7 @@
public void periodic() {
// Update the odometry in the periodic block
m_odometry.update(Rotation2d.fromDegrees(getHeading()), m_leftEncoder.getDistance(),
- m_rightEncoder.getDistance());
+ m_rightEncoder.getDistance());
}
/**
@@ -183,7 +176,7 @@
* @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);
+ return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
}
/**
@@ -192,6 +185,6 @@
* @return The turn rate of the robot, in degrees per second
*/
public double getTurnRate() {
- return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0);
+ return m_gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
}
}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java
index 922b8ff..570a0ad 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java
@@ -18,8 +18,9 @@
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj.examples.schedulereventlogging.Constants.OIConstants;
+
import static edu.wpi.first.wpilibj.XboxController.Button;
-import static edu.wpi.first.wpilibj.examples.schedulereventlogging.Constants.OIConstants.kDriverControllerPort;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -29,7 +30,8 @@
*/
public class RobotContainer {
// The driver's controller
- private final XboxController m_driverController = new XboxController(kDriverControllerPort);
+ private final XboxController m_driverController =
+ new XboxController(OIConstants.kDriverControllerPort);
// A few commands that do nothing, but will demonstrate the scheduler functionality
private final CommandBase m_instantCommand1 = new InstantCommand();
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
index ff9f28b..35afdf3 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
@@ -11,9 +11,6 @@
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
-import static edu.wpi.first.wpilibj.examples.swervebot.Drivetrain.kMaxAngularSpeed;
-import static edu.wpi.first.wpilibj.examples.swervebot.Drivetrain.kMaxSpeed;
-
public class Robot extends TimedRobot {
private final XboxController m_controller = new XboxController(0);
private final Drivetrain m_swerve = new Drivetrain();
@@ -32,18 +29,18 @@
private void driveWithJoystick(boolean fieldRelative) {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
- final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * kMaxSpeed;
+ final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * Drivetrain.kMaxSpeed;
// Get the y speed or sideways/strafe speed. We are inverting this because
// we want a positive value when we pull to the left. Xbox controllers
// return positive values when you pull to the right by default.
- final var ySpeed = -m_controller.getX(GenericHID.Hand.kLeft) * kMaxSpeed;
+ final var ySpeed = -m_controller.getX(GenericHID.Hand.kLeft) * Drivetrain.kMaxSpeed;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
- final var rot = -m_controller.getX(GenericHID.Hand.kRight) * kMaxAngularSpeed;
+ final var rot = -m_controller.getX(GenericHID.Hand.kRight) * Drivetrain.kMaxAngularSpeed;
m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative);
}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java
index 19fc601..706b15e 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java
@@ -74,6 +74,7 @@
public static final double kvVoltSecondsPerMeter = 0.8;
public static final double kaVoltSecondsSquaredPerMeter = 0.15;
+ public static final double kMaxSpeedMetersPerSecond = 3;
}
public static final class ModuleConstants {
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
index d0fcc71..503cede 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
@@ -24,17 +24,11 @@
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants;
+import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.OIConstants;
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}
@@ -46,7 +40,7 @@
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
// The driver's controller
- XboxController m_driverController = new XboxController(kDriverControllerPort);
+ XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -85,9 +79,10 @@
public Command getAutonomousCommand() {
// Create config for trajectory
TrajectoryConfig config =
- new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared)
+ new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
+ AutoConstants.kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
- .setKinematics(kDriveKinematics);
+ .setKinematics(DriveConstants.kDriveKinematics);
// An example trajectory to follow. All units in meters.
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
@@ -96,7 +91,7 @@
// Pass through these two interior waypoints, making an 's' curve path
List.of(
new Translation2d(1, 1),
- new Translation2d(2, - 1)
+ new Translation2d(2, -1)
),
// End 3 meters straight ahead of where we started, facing forward
new Pose2d(3, 0, new Rotation2d(0)),
@@ -106,12 +101,13 @@
SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
exampleTrajectory,
m_robotDrive::getPose, //Functional interface to feed supplier
- kDriveKinematics,
+ DriveConstants.kDriveKinematics,
//Position controllers
- new PIDController(kPXController, 0, 0),
- new PIDController(kPYController, 0, 0),
- new ProfiledPIDController(kPThetaController, 0, 0, kThetaControllerConstraints),
+ new PIDController(AutoConstants.kPXController, 0, 0),
+ new PIDController(AutoConstants.kPYController, 0, 0),
+ new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0,
+ AutoConstants.kThetaControllerConstraints),
m_robotDrive::setModuleStates,
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
index ba80a15..5d9cf56 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
@@ -17,77 +17,56 @@
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;
+import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants;
@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_frontLeft
+ = new SwerveModule(DriveConstants.kFrontLeftDriveMotorPort,
+ DriveConstants.kFrontLeftTurningMotorPort,
+ DriveConstants.kFrontLeftDriveEncoderPorts,
+ DriveConstants.kFrontLeftTurningEncoderPorts,
+ DriveConstants.kFrontLeftDriveEncoderReversed,
+ DriveConstants.kFrontLeftTurningEncoderReversed);
- private final SwerveModule m_rearLeft = new SwerveModule(kRearLeftDriveMotorPort,
- kRearLeftTurningMotorPort,
- kRearLeftDriveEncoderPorts,
- kRearLeftTurningEncoderPorts,
- kRearLeftDriveEncoderReversed,
- kRearLeftTurningEncoderReversed);
+ private final SwerveModule m_rearLeft =
+ new SwerveModule(DriveConstants.kRearLeftDriveMotorPort,
+ DriveConstants.kRearLeftTurningMotorPort,
+ DriveConstants.kRearLeftDriveEncoderPorts,
+ DriveConstants.kRearLeftTurningEncoderPorts,
+ DriveConstants.kRearLeftDriveEncoderReversed,
+ DriveConstants.kRearLeftTurningEncoderReversed);
- private final SwerveModule m_frontRight = new SwerveModule(kFrontRightDriveMotorPort,
- kFrontRightTurningMotorPort,
- kFrontRightDriveEncoderPorts,
- kFrontRightTurningEncoderPorts,
- kFrontRightDriveEncoderReversed,
- kFrontRightTurningEncoderReversed);
+ private final SwerveModule m_frontRight =
+ new SwerveModule(DriveConstants.kFrontRightDriveMotorPort,
+ DriveConstants.kFrontRightTurningMotorPort,
+ DriveConstants.kFrontRightDriveEncoderPorts,
+ DriveConstants.kFrontRightTurningEncoderPorts,
+ DriveConstants.kFrontRightDriveEncoderReversed,
+ DriveConstants.kFrontRightTurningEncoderReversed);
- private final SwerveModule m_rearRight = new SwerveModule(kRearRightDriveMotorPort,
- kRearRightTurningMotorPort,
- kRearRightDriveEncoderPorts,
- kRearRightTurningEncoderPorts,
- kRearRightDriveEncoderReversed,
- kRearRightTurningEncoderReversed);
+ private final SwerveModule m_rearRight =
+ new SwerveModule(DriveConstants.kRearRightDriveMotorPort,
+ DriveConstants.kRearRightTurningMotorPort,
+ DriveConstants.kRearRightDriveEncoderPorts,
+ DriveConstants.kRearRightTurningEncoderPorts,
+ DriveConstants.kRearRightDriveEncoderReversed,
+ DriveConstants.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());
+ new SwerveDriveOdometry(DriveConstants.kDriveKinematics, getAngle());
/**
* Creates a new DriveSubsystem.
*/
- public DriveSubsystem() {}
+ public DriveSubsystem() {
+ }
/**
* Returns the angle of the robot as a Rotation2d.
@@ -96,7 +75,7 @@
*/
public Rotation2d getAngle() {
// Negating the angle because WPILib gyros are CW positive.
- return Rotation2d.fromDegrees(m_gyro.getAngle() * (kGyroReversed ? 1.0 : -1.0));
+ return Rotation2d.fromDegrees(m_gyro.getAngle() * (DriveConstants.kGyroReversed ? 1.0 : -1.0));
}
@Override
@@ -138,12 +117,13 @@
*/
@SuppressWarnings("ParameterName")
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
- var swerveModuleStates = kDriveKinematics.toSwerveModuleStates(
+ var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, getAngle())
: new ChassisSpeeds(xSpeed, ySpeed, rot)
);
- SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeedMetersPerSecond);
+ SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates,
+ DriveConstants.kMaxSpeedMetersPerSecond);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);
m_rearLeft.setDesiredState(swerveModuleStates[2]);
@@ -151,12 +131,13 @@
}
/**
- * Sets the swerve ModuleStates.
- *
- * @param desiredStates The desired SwerveModule states.
- */
+ * Sets the swerve ModuleStates.
+ *
+ * @param desiredStates The desired SwerveModule states.
+ */
public void setModuleStates(SwerveModuleState[] desiredStates) {
- SwerveDriveKinematics.normalizeWheelSpeeds(desiredStates, kMaxSpeedMetersPerSecond);
+ SwerveDriveKinematics.normalizeWheelSpeeds(desiredStates,
+ DriveConstants.kMaxSpeedMetersPerSecond);
m_frontLeft.setDesiredState(desiredStates[0]);
m_frontRight.setDesiredState(desiredStates[1]);
m_rearLeft.setDesiredState(desiredStates[2]);
@@ -186,7 +167,7 @@
* @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);
+ return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
}
/**
@@ -195,6 +176,6 @@
* @return The turn rate of the robot, in degrees per second
*/
public double getTurnRate() {
- return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0);
+ return m_gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
}
}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java
index bf33288..e70c6ba 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java
@@ -15,12 +15,7 @@
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;
+import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants;
public class SwerveModule {
private final Spark m_driveMotor;
@@ -30,13 +25,15 @@
private final Encoder m_turningEncoder;
private final PIDController m_drivePIDController =
- new PIDController(kPModuleDriveController, 0, 0);
+ new PIDController(ModuleConstants.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));
+ = new ProfiledPIDController(
+ ModuleConstants.kPModuleTurningController, 0, 0,
+ new TrapezoidProfile.Constraints(
+ ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond,
+ ModuleConstants.kMaxModuleAngularAccelerationRadiansPerSecondSquared));
/**
* Constructs a SwerveModule.
@@ -45,11 +42,11 @@
* @param turningMotorChannel ID for the turning motor.
*/
public SwerveModule(int driveMotorChannel,
- int turningMotorChannel,
- int[] driveEncoderPorts,
- int[] turningEncoderPorts,
- boolean driveEncoderReversed,
- boolean turningEncoderReversed) {
+ int turningMotorChannel,
+ int[] driveEncoderPorts,
+ int[] turningEncoderPorts,
+ boolean driveEncoderReversed,
+ boolean turningEncoderReversed) {
m_driveMotor = new Spark(driveMotorChannel);
m_turningMotor = new Spark(turningMotorChannel);
@@ -61,7 +58,7 @@
// 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);
+ m_driveEncoder.setDistancePerPulse(ModuleConstants.kDriveEncoderDistancePerPulse);
//Set whether drive encoder should be reversed or not
m_driveEncoder.setReverseDirection(driveEncoderReversed);
@@ -69,7 +66,7 @@
// 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);
+ m_turningEncoder.setDistancePerPulse(ModuleConstants.kTurningEncoderDistancePerPulse);
//Set whether turning encoder should be reversed or not
m_turningEncoder.setReverseDirection(turningEncoderReversed);
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java
index cc79077..7cfe642 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java
@@ -24,6 +24,28 @@
*/
public ExampleCommand(ExampleSubsystem subsystem) {
m_subsystem = subsystem;
+ // Use addRequirements() here to declare subsystem dependencies.
addRequirements(subsystem);
}
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
}
diff --git a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
index b260c77..8d56fcf 100644
--- a/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
+++ b/third_party/allwpilib_2019/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
@@ -17,11 +17,8 @@
}
- /**
- * Will be called periodically whenever the CommandScheduler runs.
- */
@Override
public void periodic() {
-
+ // This method will be called once per scheduler run
}
}
diff --git a/third_party/allwpilib_2019/wpiutil/CMakeLists.txt b/third_party/allwpilib_2019/wpiutil/CMakeLists.txt
index 93e0191..559c8ed 100644
--- a/third_party/allwpilib_2019/wpiutil/CMakeLists.txt
+++ b/third_party/allwpilib_2019/wpiutil/CMakeLists.txt
@@ -62,7 +62,10 @@
set(CMAKE_JAVA_INCLUDE_PATH wpiutil.jar ${EJML_JARS})
- execute_process(COMMAND python3 ${CMAKE_SOURCE_DIR}/wpiutil/generate_numbers.py ${CMAKE_BINARY_DIR}/wpiutil)
+ execute_process(COMMAND python3 ${CMAKE_SOURCE_DIR}/wpiutil/generate_numbers.py ${CMAKE_BINARY_DIR}/wpiutil RESULT_VARIABLE generateResult)
+ if(NOT (generateResult EQUAL "0"))
+ message(FATAL_ERROR "python3 generate_numbers.py failed")
+ endif()
set(CMAKE_JNI_TARGET true)
diff --git a/third_party/allwpilib_2019/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java b/third_party/allwpilib_2019/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
index cf57834..bd555dc 100644
--- a/third_party/allwpilib_2019/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
+++ b/third_party/allwpilib_2019/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
@@ -19,6 +19,17 @@
* @param low The lower boundary to which to clamp value.
* @param high The higher boundary to which to clamp value.
*/
+ public static int clamp(int value, int low, int high) {
+ return Math.max(low, Math.min(value, high));
+ }
+
+ /**
+ * Returns value clamped between low and high boundaries.
+ *
+ * @param value Value to clamp.
+ * @param low The lower boundary to which to clamp value.
+ * @param high The higher boundary to which to clamp value.
+ */
public static double clamp(double value, double low, double high) {
return Math.max(low, Math.min(value, high));
}
diff --git a/third_party/allwpilib_2019/wpiutil/src/main/native/cpp/leb128.cpp b/third_party/allwpilib_2019/wpiutil/src/main/native/cpp/leb128.cpp
index ce68ed8..202ee9a 100644
--- a/third_party/allwpilib_2019/wpiutil/src/main/native/cpp/leb128.cpp
+++ b/third_party/allwpilib_2019/wpiutil/src/main/native/cpp/leb128.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. */
@@ -38,7 +38,7 @@
}
uint64_t ReadUleb128(const char* addr, uint64_t* ret) {
- uint32_t result = 0;
+ uint64_t result = 0;
int shift = 0;
size_t count = 0;
@@ -59,7 +59,7 @@
}
bool ReadUleb128(raw_istream& is, uint64_t* ret) {
- uint32_t result = 0;
+ uint64_t result = 0;
int shift = 0;
while (1) {
diff --git a/third_party/allwpilib_2019/wpiutil/src/main/native/cpp/llvm/ManagedStatic.cpp b/third_party/allwpilib_2019/wpiutil/src/main/native/cpp/llvm/ManagedStatic.cpp
index 0cfe58a..a8c82bf 100644
--- a/third_party/allwpilib_2019/wpiutil/src/main/native/cpp/llvm/ManagedStatic.cpp
+++ b/third_party/allwpilib_2019/wpiutil/src/main/native/cpp/llvm/ManagedStatic.cpp
@@ -30,6 +30,22 @@
return ManagedStaticMutex;
}
+void ManagedStaticBase::RegisterManagedStatic(void* created,
+ void (*Deleter)(void*)) const {
+ std::scoped_lock Lock(*getManagedStaticMutex());
+
+ if (!Ptr.load(std::memory_order_relaxed)) {
+ void *Tmp = created;
+
+ Ptr.store(Tmp, std::memory_order_release);
+ DeleterFn = Deleter;
+
+ // Add to list of managed statics.
+ Next = StaticList;
+ StaticList = this;
+ }
+}
+
void ManagedStaticBase::RegisterManagedStatic(void *(*Creator)(),
void (*Deleter)(void*)) const {
assert(Creator);
diff --git a/third_party/allwpilib_2019/wpiutil/src/main/native/include/wpi/ManagedStatic.h b/third_party/allwpilib_2019/wpiutil/src/main/native/include/wpi/ManagedStatic.h
index b99ad66..28c11fa 100644
--- a/third_party/allwpilib_2019/wpiutil/src/main/native/include/wpi/ManagedStatic.h
+++ b/third_party/allwpilib_2019/wpiutil/src/main/native/include/wpi/ManagedStatic.h
@@ -43,6 +43,7 @@
mutable const ManagedStaticBase *Next;
void RegisterManagedStatic(void *(*creator)(), void (*deleter)(void*)) const;
+ void RegisterManagedStatic(void *created, void (*deleter)(void*)) const;
public:
/// isConstructed - Return true if this object has not been created yet.
@@ -60,6 +61,12 @@
class Deleter = object_deleter<C>>
class ManagedStatic : public ManagedStaticBase {
public:
+ ManagedStatic() = default;
+
+ ManagedStatic(C* created, void(*deleter)(void*)) {
+ RegisterManagedStatic(created, deleter);
+ }
+
// Accessors.
C &operator*() {
void *Tmp = Ptr.load(std::memory_order_acquire);
diff --git a/third_party/allwpilib_2019/wpiutil/src/test/native/ManagedStaticTest.cpp b/third_party/allwpilib_2019/wpiutil/src/test/native/ManagedStaticTest.cpp
new file mode 100644
index 0000000..81a4b9c
--- /dev/null
+++ b/third_party/allwpilib_2019/wpiutil/src/test/native/ManagedStaticTest.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 "wpi/ManagedStatic.h" // NOLINT(build/include_order)
+
+#include "gtest/gtest.h"
+
+static int refCount = 0;
+
+struct StaticTestClass {
+ StaticTestClass() { refCount++; }
+ ~StaticTestClass() { refCount--; }
+
+ void Func() {}
+};
+
+namespace wpi {
+TEST(ManagedStaticTest, LazyDoesNotInitialize) {
+ {
+ refCount = 0;
+ wpi::ManagedStatic<StaticTestClass> managedStatic;
+ ASSERT_EQ(refCount, 0);
+ }
+ ASSERT_EQ(refCount, 0);
+ wpi_shutdown();
+}
+
+TEST(ManagedStaticTest, LazyInitDoesntDestruct) {
+ {
+ refCount = 0;
+ wpi::ManagedStatic<StaticTestClass> managedStatic;
+ ASSERT_EQ(refCount, 0);
+ managedStatic->Func();
+ ASSERT_EQ(refCount, 1);
+ }
+ ASSERT_EQ(refCount, 1);
+ wpi_shutdown();
+ ASSERT_EQ(refCount, 0);
+}
+
+TEST(ManagedStaticTest, EagerInit) {
+ {
+ refCount = 0;
+ StaticTestClass* test = new StaticTestClass{};
+ ASSERT_EQ(refCount, 1);
+ wpi::ManagedStatic<StaticTestClass> managedStatic(
+ test, [](void* val) { delete static_cast<StaticTestClass*>(val); });
+ ASSERT_EQ(refCount, 1);
+ managedStatic->Func();
+ ASSERT_EQ(refCount, 1);
+ }
+ ASSERT_EQ(refCount, 1);
+ wpi_shutdown();
+ ASSERT_EQ(refCount, 0);
+}
+} // namespace wpi