Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-26 01:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

[command] Remove old command-based framework (#4211)

Browse Source
This commit is contained in:
Tyler Veness
2022-05-04 22:02:53 -07:00
committed by GitHub
parent ee03a7ad3b
commit 99343d40ba
171 changed files with 32 additions and 12798 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
CMakeLists.txt
View File

@@ -54,7 +54,6 @@ option(WITH_JAVA "Include java and JNI in the build" ON)
option(WITH_CSCORE "Build cscore (needs OpenCV)" ON)
option(WITH_WPIMATH "Build wpimath" ON)
option(WITH_WPILIB "Build hal, wpilibc/j, and myRobot (needs OpenCV)" ON)
option(WITH_OLD_COMMANDS "Build old commands" OFF)
option(WITH_EXAMPLES "Build examples" OFF)
option(WITH_TESTS "Build unit tests (requires internet connection)" ON)
option(WITH_GUI "Build GUI items" ON)
@@ -156,7 +155,6 @@ set(HAL_DEP_REPLACE_IMPL "include(\${SELF_DIR}/hal-config.cmake)")
set(WPIMATH_DEP_REPLACE "include($\{SELF_DIR\}/wpimath-config.cmake)")
set(WPILIBC_DEP_REPLACE_IMPL "include(\${SELF_DIR}/wpilibc-config.cmake)")
set(WPILIBNEWCOMMANDS_DEP_REPLACE "include(\${SELF_DIR}/wpilibNewcommands-config.cmake)")
set(WPILIBOLDCOMMANDS_DEP_REPLACE "include(\${SELF_DIR}/wpilibOldcommands-config.cmake)")
else()
set(WPIUTIL_DEP_REPLACE "find_dependency(wpiutil)")
set(NTCORE_DEP_REPLACE "find_dependency(ntcore)")
@@ -166,7 +164,6 @@ set(HAL_DEP_REPLACE_IMPL "find_dependency(hal)")
set(WPIMATH_DEP_REPLACE "find_dependency(wpimath)")
set(WPILIBC_DEP_REPLACE_IMPL "find_dependency(wpilibc)")
set(WPILIBNEWCOMMANDS_DEP_REPLACE "find_dependency(wpilibNewCommands)")
set(WPILIBOLDCOMMANDS_DEP_REPLACE "find_dependency(wpilibOldCommands)")
endif()
set(FILENAME_DEP_REPLACE "get_filename_component(SELF_DIR \"$\{CMAKE_CURRENT_LIST_FILE\}\" PATH)")
@@ -283,9 +280,6 @@ if (WITH_WPILIB)
add_subdirectory(wpilibj)
add_subdirectory(wpilibc)
add_subdirectory(wpilibNewCommands)
if (WITH_OLD_COMMANDS)
add_subdirectory(wpilibOldCommands)
endif()
if (WITH_EXAMPLES)
add_subdirectory(wpilibcExamples)
endif()

8
MavenArtifacts.md
View File

@@ -134,14 +134,6 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
* wpimath
* wpiutil
* wpilibOldCommands
* wpilibc
* hal
* cameraserver
* ntcore
* cscore
* wpiutil
### Third Party Artifacts

2
README.md
View File

@@ -79,7 +79,7 @@ If opening from a fresh clone, generated java dependencies will not exist. Most
`./gradlew testDesktopCpp` and `./gradlew testDesktopJava` will build and run the tests for `wpilibc` and `wpilibj` respectively. They will only build the minimum components required to run the tests.
`testDesktopCpp` and `testDesktopJava` tasks also exist for the projects `wpiutil`, `ntcore`, `cscore`, `hal` `wpilibOldCommands`, `wpilibNewCommands` and `cameraserver`. These can be ran with `./gradlew :projectName:task`.
`testDesktopCpp` and `testDesktopJava` tasks also exist for the projects `wpiutil`, `ntcore`, `cscore`, `hal` `wpilibNewCommands` and `cameraserver`. These can be ran with `./gradlew :projectName:task`.
`./gradlew buildDesktopCpp` and `./gradlew buildDesktopJava` will compile `wpilibcExamples` and `wpilibjExamples` respectively. The results can't be ran, but they can compile.

3
docs/build.gradle
View File

@@ -11,7 +11,6 @@ evaluationDependsOn(':cameraserver')
evaluationDependsOn(':wpimath')
evaluationDependsOn(':wpilibc')
evaluationDependsOn(':wpilibj')
evaluationDependsOn(':wpilibOldCommands')
evaluationDependsOn(':wpilibNewCommands')
def baseArtifactIdCpp = 'documentation'
@@ -34,7 +33,6 @@ cppProjectZips.add(project(':cscore').cppHeadersZip)
cppProjectZips.add(project(':cameraserver').cppHeadersZip)
cppProjectZips.add(project(':wpimath').cppHeadersZip)
cppProjectZips.add(project(':wpilibc').cppHeadersZip)
cppProjectZips.add(project(':wpilibOldCommands').cppHeadersZip)
cppProjectZips.add(project(':wpilibNewCommands').cppHeadersZip)
doxygen {
@@ -204,7 +202,6 @@ task generateJavaDocs(type: Javadoc) {
source project(':wpimath').sourceSets.main.java
source project(':wpilibj').sourceSets.main.java
source project(':cameraserver').sourceSets.main.java
source project(':wpilibOldCommands').sourceSets.main.java
source project(':wpilibNewCommands').sourceSets.main.java
source configurations.javaSource.collect { zipTree(it) }
include '**/*.java'

3
myRobot/build.gradle
View File

@@ -47,7 +47,6 @@ dependencies {
implementation project(':ntcore')
implementation project(':cscore')
implementation project(':cameraserver')
implementation project(':wpilibOldCommands')
implementation project(':wpilibNewCommands')
}
@@ -161,7 +160,6 @@ model {
deploy.targets.roborio.artifacts.myRobotCpp.binary = binary
}
}
lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
@@ -208,7 +206,6 @@ model {
deploy.targets.roborio.artifacts.myRobotCppStatic.binary = binary
}
}
lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'static'
lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'static'
lib project: ':wpilibc', library: 'wpilibc', linkage: 'static'
lib project: ':wpimath', library: 'wpimath', linkage: 'static'

1
settings.gradle
View File

@@ -44,7 +44,6 @@ include 'simulation:halsim_ws_client'
include 'simulation:halsim_ws_server'
include 'cameraserver'
include 'cameraserver:multiCameraServer'
include 'wpilibOldCommands'
include 'wpilibNewCommands'
include 'myRobot'
include 'docs'

2
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
View File

@@ -17,8 +17,6 @@ import java.util.function.BooleanSupplier;
* <p>This class represents a subclass of Trigger that is specifically aimed at buttons on an
* operator interface as a common use case of the more generalized Trigger objects. This is a simple
* wrapper around Trigger with the method names renamed to fit the Button object use.
*
* <p>This class is provided by the OldCommands VendorDep
*/
public class Button extends Trigger {
/**

59
wpilibOldCommands/CMakeLists.txt
View File

@@ -1,59 +0,0 @@
project(wpilibOldCommands)
include(SubDirList)
include(CompileWarnings)
include(AddTest)
if (WITH_JAVA)
find_package(Java REQUIRED)
include(UseJava)
set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
add_jar(wpilibOldCommands_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME wpilibOldCommands)
get_property(WPIlIBOLDCOMMANDS_JAR_FILE TARGET wpilibOldCommands_jar PROPERTY JAR_FILE)
install(FILES ${WPIlIBOLDCOMMANDS_JAR_FILE} DESTINATION "${java_lib_dest}")
set_property(TARGET wpilibOldCommands_jar PROPERTY FOLDER "java")
if (WITH_FLAT_INSTALL)
set (wpilibOldCommands_config_dir ${wpilib_dest})
else()
set (wpilibOldCommands_config_dir share/wpilibOldCommands)
endif()
install(FILES wpilibOldCommands-config.cmake DESTINATION ${wpilibOldCommands_config_dir})
endif()
file(GLOB_RECURSE wpilibOldCommands_native_src src/main/native/cpp/*.cpp)
add_library(wpilibOldCommands ${wpilibOldCommands_native_src})
set_target_properties(wpilibOldCommands PROPERTIES DEBUG_POSTFIX "d")
set_property(TARGET wpilibOldCommands PROPERTY FOLDER "libraries")
target_compile_features(wpilibOldCommands PUBLIC cxx_std_17)
wpilib_target_warnings(wpilibOldCommands)
target_link_libraries(wpilibOldCommands wpilibc)
target_include_directories(wpilibOldCommands PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
$<INSTALL_INTERFACE:${include_dest}/wpilibOldCommands>)
install(TARGETS wpilibOldCommands EXPORT wpilibOldCommands DESTINATION "${main_lib_dest}")
install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpilibOldCommands")
if (MSVC OR FLAT_INSTALL_WPILIB)
set(wpilibOldCommands_config_dir ${wpilib_dest})
else()
set(wpilibOldCommands_config_dir share/wpilibOldCommands)
endif()
configure_file(wpilibOldCommands-config.cmake.in ${WPILIB_BINARY_DIR}/wpilibOldCommands-config.cmake)
install(FILES ${WPILIB_BINARY_DIR}/wpilibOldCommands-config.cmake DESTINATION ${wpilibOldCommands_config_dir})
install(EXPORT wpilibOldCommands DESTINATION ${wpilibOldCommands_config_dir})
if (WITH_TESTS)
wpilib_add_test(wpilibOldCommands src/test/native/cpp)
target_include_directories(wpilibOldCommands_test PRIVATE src/test/native/include)
target_link_libraries(wpilibOldCommands_test wpilibOldCommands gmock_main)
endif()

37
wpilibOldCommands/WPILibOldCommands.json
View File

@@ -1,37 +0,0 @@
{
"fileName": "WPILibOldCommands.json",
"name": "WPILib-Old-Commands",
"version": "1.0.0",
"uuid": "b066afc2-5c18-43c4-b758-43381fcb275e",
"mavenUrls": [],
"jsonUrl": "",
"javaDependencies": [
{
"groupId": "edu.wpi.first.wpilibOldCommands",
"artifactId": "wpilibOldCommands-java",
"version": "wpilib"
}
],
"jniDependencies": [],
"cppDependencies": [
{
"groupId": "edu.wpi.first.wpilibOldCommands",
"artifactId": "wpilibOldCommands-cpp",
"version": "wpilib",
"libName": "wpilibOldCommands",
"headerClassifier": "headers",
"sourcesClassifier": "sources",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
"binaryPlatforms": [
"linuxathena",
"linuxraspbian",
"linuxaarch64bionic",
"windowsx86-64",
"windowsx86",
"linuxx86-64",
"osxx86-64"
]
}
]
}

119
wpilibOldCommands/build.gradle
View File

@@ -1,119 +0,0 @@
ext {
nativeName = 'wpilibOldCommands'
devMain = 'edu.wpi.first.wpilibj.commands.DevMain'
}
evaluationDependsOn(':ntcore')
evaluationDependsOn(':cscore')
evaluationDependsOn(':hal')
evaluationDependsOn(':wpimath')
evaluationDependsOn(':wpilibc')
evaluationDependsOn(':cameraserver')
evaluationDependsOn(':wpilibj')
apply from: "${rootDir}/shared/javacpp/setupBuild.gradle"
dependencies {
implementation project(':wpiutil')
implementation project(':ntcore')
implementation project(':cscore')
implementation project(':hal')
implementation project(':wpimath')
implementation project(':wpilibj')
devImplementation project(':wpiutil')
devImplementation project(':ntcore')
devImplementation project(':cscore')
devImplementation project(':hal')
devImplementation project(':wpimath')
devImplementation project(':wpilibj')
}
nativeUtils.exportsConfigs {
wpilibOldCommands {
x86ExcludeSymbols = [
'_CT??_R0?AV_System_error',
'_CT??_R0?AVexception',
'_CT??_R0?AVfailure',
'_CT??_R0?AVruntime_error',
'_CT??_R0?AVsystem_error',
'_CTA5?AVfailure',
'_TI5?AVfailure',
'_CT??_R0?AVout_of_range',
'_CTA3?AVout_of_range',
'_TI3?AVout_of_range',
'_CT??_R0?AVbad_cast'
]
x64ExcludeSymbols = [
'_CT??_R0?AV_System_error',
'_CT??_R0?AVexception',
'_CT??_R0?AVfailure',
'_CT??_R0?AVruntime_error',
'_CT??_R0?AVsystem_error',
'_CTA5?AVfailure',
'_TI5?AVfailure',
'_CT??_R0?AVout_of_range',
'_CTA3?AVout_of_range',
'_TI3?AVout_of_range',
'_CT??_R0?AVbad_cast'
]
}
}
model {
components {}
binaries {
all {
if (!it.buildable || !(it instanceof NativeBinarySpec)) {
return
}
lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
project(':hal').addHalDependency(it, 'shared')
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
if (it.component.name == "${nativeName}Dev") {
lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
project(':hal').addHalJniDependency(it)
}
if (it instanceof GoogleTestTestSuiteBinarySpec) {
nativeUtils.useRequiredLibrary(it, 'opencv_shared')
lib project: ':cscore', library: 'cscore', linkage: 'shared'
}
if ((it instanceof NativeExecutableBinarySpec || it instanceof GoogleTestTestSuiteBinarySpec) && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
}
}
}
tasks {
def c = $.components
def found = false
def systemArch = getCurrentArch()
c.each {
if (it in NativeExecutableSpec && it.name == "${nativeName}Dev") {
it.binaries.each {
if (!found) {
def arch = it.targetPlatform.name
if (arch == systemArch) {
def filePath = it.tasks.install.installDirectory.get().toString() + File.separatorChar + 'lib'
found = true
}
}
}
}
}
}
}
test {
testLogging {
outputs.upToDateWhen {false}
showStandardStreams = true
}
}
tasks.withType(JavaCompile) {
options.compilerArgs += "-Xlint:-removal"
}

21
wpilibOldCommands/src/dev/java/edu/wpi/first/wpilibj/commands/DevMain.java
View File

@@ -1,21 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands;
import edu.wpi.first.hal.HALUtil;
import edu.wpi.first.networktables.NetworkTablesJNI;
import edu.wpi.first.util.RuntimeDetector;
public final class DevMain {
/** Main entry point. */
public static void main(String[] args) {
System.out.println("Hello World!");
System.out.println(RuntimeDetector.getPlatformPath());
System.out.println(NetworkTablesJNI.now());
System.out.println(HALUtil.getHALRuntimeType());
}
private DevMain() {}
}

5
wpilibOldCommands/src/dev/native/cpp/main.cpp
View File

@@ -1,5 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
int main() {}

817
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDBase.java
View File

@@ -1,817 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.util.BoundaryException;
import edu.wpi.first.math.filter.LinearFilter;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
import java.util.concurrent.locks.ReentrantLock;
/**
* Class implements a PID Control Loop.
*
* <p>Creates a separate thread which reads the given PIDSource and takes care of the integral
* calculations, as well as writing the given PIDOutput.
*
* <p>This feedback controller runs in discrete time, so time deltas are not used in the integral
* and derivative calculations. Therefore, the sample rate affects the controller's behavior for a
* given set of PID constants.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @deprecated All APIs which use this have been deprecated.
*/
@Deprecated(since = "2020", forRemoval = true)
public class PIDBase implements PIDInterface, PIDOutput, Sendable, AutoCloseable {
public static final double kDefaultPeriod = 0.05;
private static int instances;
// Factor for "proportional" control
@SuppressWarnings("MemberName")
private double m_P;
// Factor for "integral" control
@SuppressWarnings("MemberName")
private double m_I;
// Factor for "derivative" control
@SuppressWarnings("MemberName")
private double m_D;
// Factor for "feed forward" control
@SuppressWarnings("MemberName")
private double m_F;
// |maximum output|
private double m_maximumOutput = 1.0;
// |minimum output|
private double m_minimumOutput = -1.0;
// Maximum input - limit setpoint to this
private double m_maximumInput;
// Minimum input - limit setpoint to this
private double m_minimumInput;
// Input range - difference between maximum and minimum
private double m_inputRange;
// Do the endpoints wrap around? (e.g., absolute encoder)
private boolean m_continuous;
// Is the PID controller enabled
protected boolean m_enabled;
// The prior error (used to compute velocity)
private double m_prevError;
// The sum of the errors for use in the integral calc
private double m_totalError;
// The tolerance object used to check if on target
private Tolerance m_tolerance;
private double m_setpoint;
private double m_prevSetpoint;
private double m_result;
private LinearFilter m_filter;
protected ReentrantLock m_thisMutex = new ReentrantLock();
// Ensures when disable() is called, pidWrite() won't run if calculate()
// is already running at that time.
protected ReentrantLock m_pidWriteMutex = new ReentrantLock();
protected PIDSource m_pidInput;
protected PIDOutput m_pidOutput;
protected Timer m_setpointTimer;
/**
* Tolerance is the type of tolerance used to specify if the PID controller is on target.
*
* <p>The various implementations of this class such as PercentageTolerance and AbsoluteTolerance
* specify types of tolerance specifications to use.
*/
public interface Tolerance {
boolean onTarget();
}
/** Used internally for when Tolerance hasn't been set. */
public static class NullTolerance implements Tolerance {
@Override
public boolean onTarget() {
throw new IllegalStateException("No tolerance value set when calling onTarget().");
}
}
public class PercentageTolerance implements Tolerance {
private final double m_percentage;
PercentageTolerance(double value) {
m_percentage = value;
}
@Override
public boolean onTarget() {
return Math.abs(getError()) < m_percentage / 100 * m_inputRange;
}
}
public class AbsoluteTolerance implements Tolerance {
private final double m_value;
AbsoluteTolerance(double value) {
m_value = value;
}
@Override
public boolean onTarget() {
return Math.abs(getError()) < m_value;
}
}
/**
* Allocate a PID object with the given constants for P, I, D, and F.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param Kf the feed forward term
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output percentage
*/
@SuppressWarnings("ParameterName")
public PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output) {
requireNonNullParam(source, "source", "PIDBase");
requireNonNullParam(output, "output", "PIDBase");
m_setpointTimer = new Timer();
m_setpointTimer.start();
m_P = Kp;
m_I = Ki;
m_D = Kd;
m_F = Kf;
m_pidInput = source;
m_filter = LinearFilter.movingAverage(1);
m_pidOutput = output;
instances++;
HAL.report(tResourceType.kResourceType_PIDController, instances);
m_tolerance = new NullTolerance();
SendableRegistry.add(this, "PIDController", instances);
}
/**
* Allocate a PID object with the given constants for P, I, and D.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source the PIDSource object that is used to get values
* @param output the PIDOutput object that is set to the output percentage
*/
@SuppressWarnings("ParameterName")
public PIDBase(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
this(Kp, Ki, Kd, 0.0, source, output);
}
@Override
public void close() {
SendableRegistry.remove(this);
}
/**
* Read the input, calculate the output accordingly, and write to the output. This should only be
* called by the PIDTask and is created during initialization.
*/
@SuppressWarnings("LocalVariableName")
protected void calculate() {
if (m_pidInput == null || m_pidOutput == null) {
return;
}
boolean enabled;
m_thisMutex.lock();
try {
enabled = m_enabled;
} finally {
m_thisMutex.unlock();
}
if (enabled) {
double input;
// Storage for function inputs
PIDSourceType pidSourceType;
double P;
double I;
double D;
double feedForward = calculateFeedForward();
double minimumOutput;
double maximumOutput;
// Storage for function input-outputs
double prevError;
double error;
double totalError;
m_thisMutex.lock();
try {
input = m_filter.calculate(m_pidInput.pidGet());
pidSourceType = m_pidInput.getPIDSourceType();
P = m_P;
I = m_I;
D = m_D;
minimumOutput = m_minimumOutput;
maximumOutput = m_maximumOutput;
prevError = m_prevError;
error = getContinuousError(m_setpoint - input);
totalError = m_totalError;
} finally {
m_thisMutex.unlock();
}
// Storage for function outputs
double result;
if (pidSourceType.equals(PIDSourceType.kRate)) {
if (P != 0) {
totalError = clamp(totalError + error, minimumOutput / P, maximumOutput / P);
}
result = P * totalError + D * error + feedForward;
} else {
if (I != 0) {
totalError = clamp(totalError + error, minimumOutput / I, maximumOutput / I);
}
result = P * error + I * totalError + D * (error - prevError) + feedForward;
}
result = clamp(result, minimumOutput, maximumOutput);
// Ensures m_enabled check and pidWrite() call occur atomically
m_pidWriteMutex.lock();
m_thisMutex.lock();
try {
if (m_enabled) {
// Don't block other PIDController operations on pidWrite()
m_thisMutex.unlock();
m_pidOutput.pidWrite(result);
}
} finally {
if (!m_enabled) {
m_thisMutex.unlock();
}
m_pidWriteMutex.unlock();
}
m_thisMutex.lock();
try {
m_prevError = error;
m_totalError = totalError;
m_result = result;
} finally {
m_thisMutex.unlock();
}
}
}
/**
* Calculate the feed forward term.
*
* <p>Both of the provided feed forward calculations are velocity feed forwards. If a different
* feed forward calculation is desired, the user can override this function and provide his or her
* own. This function does no synchronization because the PIDController class only calls it in
* synchronized code, so be careful if calling it oneself.
*
* <p>If a velocity PID controller is being used, the F term should be set to 1 over the maximum
* setpoint for the output. If a position PID controller is being used, the F term should be set
* to 1 over the maximum speed for the output measured in setpoint units per this controller's
* update period (see the default period in this class's constructor).
*
* @return The feedforward value.
*/
protected double calculateFeedForward() {
if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) {
return m_F * getSetpoint();
} else {
double temp = m_F * getDeltaSetpoint();
m_prevSetpoint = m_setpoint;
m_setpointTimer.reset();
return temp;
}
}
/**
* Set the PID Controller gain parameters. Set the proportional, integral, and differential
* coefficients.
*
* @param p Proportional coefficient
* @param i Integral coefficient
* @param d Differential coefficient
*/
@Override
@SuppressWarnings("ParameterName")
public void setPID(double p, double i, double d) {
m_thisMutex.lock();
try {
m_P = p;
m_I = i;
m_D = d;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the PID Controller gain parameters. Set the proportional, integral, and differential
* coefficients.
*
* @param p Proportional coefficient
* @param i Integral coefficient
* @param d Differential coefficient
* @param f Feed forward coefficient
*/
@SuppressWarnings("ParameterName")
public void setPID(double p, double i, double d, double f) {
m_thisMutex.lock();
try {
m_P = p;
m_I = i;
m_D = d;
m_F = f;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the Proportional coefficient of the PID controller gain.
*
* @param p Proportional coefficient
*/
@SuppressWarnings("ParameterName")
public void setP(double p) {
m_thisMutex.lock();
try {
m_P = p;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the Integral coefficient of the PID controller gain.
*
* @param i Integral coefficient
*/
@SuppressWarnings("ParameterName")
public void setI(double i) {
m_thisMutex.lock();
try {
m_I = i;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the Differential coefficient of the PID controller gain.
*
* @param d differential coefficient
*/
@SuppressWarnings("ParameterName")
public void setD(double d) {
m_thisMutex.lock();
try {
m_D = d;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the Feed forward coefficient of the PID controller gain.
*
* @param f feed forward coefficient
*/
@SuppressWarnings("ParameterName")
public void setF(double f) {
m_thisMutex.lock();
try {
m_F = f;
} finally {
m_thisMutex.unlock();
}
}
/**
* Get the Proportional coefficient.
*
* @return proportional coefficient
*/
@Override
public double getP() {
m_thisMutex.lock();
try {
return m_P;
} finally {
m_thisMutex.unlock();
}
}
/**
* Get the Integral coefficient.
*
* @return integral coefficient
*/
@Override
public double getI() {
m_thisMutex.lock();
try {
return m_I;
} finally {
m_thisMutex.unlock();
}
}
/**
* Get the Differential coefficient.
*
* @return differential coefficient
*/
@Override
public double getD() {
m_thisMutex.lock();
try {
return m_D;
} finally {
m_thisMutex.unlock();
}
}
/**
* Get the Feed forward coefficient.
*
* @return feed forward coefficient
*/
public double getF() {
m_thisMutex.lock();
try {
return m_F;
} finally {
m_thisMutex.unlock();
}
}
/**
* Return the current PID result This is always centered on zero and constrained the the max and
* min outs.
*
* @return the latest calculated output
*/
public double get() {
m_thisMutex.lock();
try {
return m_result;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the PID controller to consider the input to be continuous, Rather then using the max and
* min input range as constraints, it considers them to be the same point and automatically
* calculates the shortest route to the setpoint.
*
* @param continuous Set to true turns on continuous, false turns off continuous
*/
public void setContinuous(boolean continuous) {
if (continuous && m_inputRange <= 0) {
throw new IllegalStateException("No input range set when calling setContinuous().");
}
m_thisMutex.lock();
try {
m_continuous = continuous;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the PID controller to consider the input to be continuous, Rather then using the max and
* min input range as constraints, it considers them to be the same point and automatically
* calculates the shortest route to the setpoint.
*/
public void setContinuous() {
setContinuous(true);
}
/**
* Sets the maximum and minimum values expected from the input and setpoint.
*
* @param minimumInput the minimum value expected from the input
* @param maximumInput the maximum value expected from the input
*/
public void setInputRange(double minimumInput, double maximumInput) {
m_thisMutex.lock();
try {
if (minimumInput > maximumInput) {
throw new BoundaryException("Lower bound is greater than upper bound");
}
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
m_inputRange = maximumInput - minimumInput;
} finally {
m_thisMutex.unlock();
}
setSetpoint(m_setpoint);
}
/**
* Sets the minimum and maximum values to write.
*
* @param minimumOutput the minimum percentage to write to the output
* @param maximumOutput the maximum percentage to write to the output
*/
public void setOutputRange(double minimumOutput, double maximumOutput) {
m_thisMutex.lock();
try {
if (minimumOutput > maximumOutput) {
throw new BoundaryException("Lower bound is greater than upper bound");
}
m_minimumOutput = minimumOutput;
m_maximumOutput = maximumOutput;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the setpoint for the PIDController.
*
* @param setpoint the desired setpoint
*/
@Override
public void setSetpoint(double setpoint) {
m_thisMutex.lock();
try {
if (m_maximumInput > m_minimumInput) {
if (setpoint > m_maximumInput) {
m_setpoint = m_maximumInput;
} else if (setpoint < m_minimumInput) {
m_setpoint = m_minimumInput;
} else {
m_setpoint = setpoint;
}
} else {
m_setpoint = setpoint;
}
} finally {
m_thisMutex.unlock();
}
}
/**
* Returns the current setpoint of the PIDController.
*
* @return the current setpoint
*/
@Override
public double getSetpoint() {
m_thisMutex.lock();
try {
return m_setpoint;
} finally {
m_thisMutex.unlock();
}
}
/**
* Returns the change in setpoint over time of the PIDController.
*
* @return the change in setpoint over time
*/
public double getDeltaSetpoint() {
m_thisMutex.lock();
try {
return (m_setpoint - m_prevSetpoint) / m_setpointTimer.get();
} finally {
m_thisMutex.unlock();
}
}
/**
* Returns the current difference of the input from the setpoint.
*
* @return the current error
*/
@Override
public double getError() {
m_thisMutex.lock();
try {
return getContinuousError(getSetpoint() - m_filter.calculate(m_pidInput.pidGet()));
} finally {
m_thisMutex.unlock();
}
}
/**
* Returns the current difference of the error over the past few iterations. You can specify the
* number of iterations to average with setToleranceBuffer() (defaults to 1). getAvgError() is
* used for the onTarget() function.
*
* @return the current average of the error
* @deprecated Use getError(), which is now already filtered.
*/
@Deprecated
public double getAvgError() {
m_thisMutex.lock();
try {
return getError();
} finally {
m_thisMutex.unlock();
}
}
/**
* Sets what type of input the PID controller will use.
*
* @param pidSource the type of input
*/
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidInput.setPIDSourceType(pidSource);
}
/**
* Returns the type of input the PID controller is using.
*
* @return the PID controller input type
*/
public PIDSourceType getPIDSourceType() {
return m_pidInput.getPIDSourceType();
}
/**
* Set the PID tolerance using a Tolerance object. Tolerance can be specified as a percentage of
* the range or as an absolute value. The Tolerance object encapsulates those options in an
* object. Use it by creating the type of tolerance that you want to use: setTolerance(new
* PIDController.AbsoluteTolerance(0.1))
*
* @param tolerance A tolerance object of the right type, e.g. PercentTolerance or
* AbsoluteTolerance
* @deprecated Use setPercentTolerance() instead.
*/
@Deprecated
public void setTolerance(Tolerance tolerance) {
m_tolerance = tolerance;
}
/**
* Set the absolute error which is considered tolerable for use with OnTarget.
*
* @param absvalue absolute error which is tolerable in the units of the input object
*/
public void setAbsoluteTolerance(double absvalue) {
m_thisMutex.lock();
try {
m_tolerance = new AbsoluteTolerance(absvalue);
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the percentage error which is considered tolerable for use with OnTarget. (Input of 15.0 =
* 15 percent)
*
* @param percentage percent error which is tolerable
*/
public void setPercentTolerance(double percentage) {
m_thisMutex.lock();
try {
m_tolerance = new PercentageTolerance(percentage);
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the number of previous error samples to average for tolerancing. When determining whether a
* mechanism is on target, the user may want to use a rolling average of previous measurements
* instead of a precise position or velocity. This is useful for noisy sensors which return a few
* erroneous measurements when the mechanism is on target. However, the mechanism will not
* register as on target for at least the specified bufLength cycles.
*
* @param bufLength Number of previous cycles to average.
* @deprecated Use a LinearFilter as the input.
*/
@Deprecated
public void setToleranceBuffer(int bufLength) {
m_thisMutex.lock();
try {
m_filter = LinearFilter.movingAverage(bufLength);
} finally {
m_thisMutex.unlock();
}
}
/**
* Return true if the error is within the percentage of the total input range, determined by
* setTolerance. This assumes that the maximum and minimum input were set using setInput.
*
* @return true if the error is less than the tolerance
*/
public boolean onTarget() {
m_thisMutex.lock();
try {
return m_tolerance.onTarget();
} finally {
m_thisMutex.unlock();
}
}
/** Reset the previous error, the integral term, and disable the controller. */
@Override
public void reset() {
m_thisMutex.lock();
try {
m_prevError = 0;
m_totalError = 0;
m_result = 0;
} finally {
m_thisMutex.unlock();
}
}
/**
* Passes the output directly to setSetpoint().
*
* <p>PIDControllers can be nested by passing a PIDController as another PIDController's output.
* In that case, the output of the parent controller becomes the input (i.e., the reference) of
* the child.
*
* <p>It is the caller's responsibility to put the data into a valid form for setSetpoint().
*/
@Override
public void pidWrite(double output) {
setSetpoint(output);
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("PIDController");
builder.setSafeState(this::reset);
builder.addDoubleProperty("p", this::getP, this::setP);
builder.addDoubleProperty("i", this::getI, this::setI);
builder.addDoubleProperty("d", this::getD, this::setD);
builder.addDoubleProperty("f", this::getF, this::setF);
builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
}
/**
* Wraps error around for continuous inputs. The original error is returned if continuous mode is
* disabled. This is an unsynchronized function.
*
* @param error The current error of the PID controller.
* @return Error for continuous inputs.
*/
protected double getContinuousError(double error) {
if (m_continuous && m_inputRange > 0) {
error %= m_inputRange;
if (Math.abs(error) > m_inputRange / 2) {
if (error > 0) {
return error - m_inputRange;
} else {
return error + m_inputRange;
}
}
}
return error;
}
private static double clamp(double value, double low, double high) {
return Math.max(low, Math.min(value, high));
}
}

182
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDController.java
View File

@@ -1,182 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
import edu.wpi.first.util.sendable.SendableBuilder;
/**
* Class implements a PID Control Loop.
*
* <p>Creates a separate thread which reads the given PIDSource and takes care of the integral
* calculations, as well as writing the given PIDOutput.
*
* <p>This feedback controller runs in discrete time, so time deltas are not used in the integral
* and derivative calculations. Therefore, the sample rate affects the controller's behavior for a
* given set of PID constants.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @deprecated Use {@link edu.wpi.first.math.controller.PIDController} instead.
*/
@Deprecated(since = "2020", forRemoval = true)
public class PIDController extends PIDBase implements Controller {
Notifier m_controlLoop = new Notifier(this::calculate);
/**
* Allocate a PID object with the given constants for P, I, D, and F.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param Kf the feed forward term
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output percentage
* @param period the loop time for doing calculations in seconds. This particularly affects
* calculations of the integral and differential terms. The default is 0.05 (50ms).
*/
@SuppressWarnings("ParameterName")
public PIDController(
double Kp,
double Ki,
double Kd,
double Kf,
PIDSource source,
PIDOutput output,
double period) {
super(Kp, Ki, Kd, Kf, source, output);
m_controlLoop.startPeriodic(period);
}
/**
* Allocate a PID object with the given constants for P, I, D and period.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source the PIDSource object that is used to get values
* @param output the PIDOutput object that is set to the output percentage
* @param period the loop time for doing calculations in seconds. This particularly affects
* calculations of the integral and differential terms. The default is 0.05 (50ms).
*/
@SuppressWarnings("ParameterName")
public PIDController(
double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period) {
this(Kp, Ki, Kd, 0.0, source, output, period);
}
/**
* Allocate a PID object with the given constants for P, I, D, using a 50ms period.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output percentage
*/
@SuppressWarnings("ParameterName")
public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
this(Kp, Ki, Kd, source, output, kDefaultPeriod);
}
/**
* Allocate a PID object with the given constants for P, I, D, using a 50ms period.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param Kf the feed forward term
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output percentage
*/
@SuppressWarnings("ParameterName")
public PIDController(
double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output) {
this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod);
}
@Override
public void close() {
m_controlLoop.close();
m_thisMutex.lock();
try {
m_pidOutput = null;
m_pidInput = null;
m_controlLoop = null;
} finally {
m_thisMutex.unlock();
}
}
/** Begin running the PIDController. */
@Override
public void enable() {
m_thisMutex.lock();
try {
m_enabled = true;
} finally {
m_thisMutex.unlock();
}
}
/** Stop running the PIDController, this sets the output to zero before stopping. */
@Override
public void disable() {
// Ensures m_enabled check and pidWrite() call occur atomically
m_pidWriteMutex.lock();
try {
m_thisMutex.lock();
try {
m_enabled = false;
} finally {
m_thisMutex.unlock();
}
m_pidOutput.pidWrite(0);
} finally {
m_pidWriteMutex.unlock();
}
}
/**
* Set the enabled state of the PIDController.
*
* @param enable True to enable the PIDController.
*/
public void setEnabled(boolean enable) {
if (enable) {
enable();
} else {
disable();
}
}
/**
* Return true if PIDController is enabled.
*
* @return True if PIDController is enabled.
*/
public boolean isEnabled() {
m_thisMutex.lock();
try {
return m_enabled;
} finally {
m_thisMutex.unlock();
}
}
/** Reset the previous error, the integral term, and disable the controller. */
@Override
public void reset() {
disable();
super.reset();
}
@Override
public void initSendable(SendableBuilder builder) {
super.initSendable(builder);
builder.addBooleanProperty("enabled", this::isEnabled, this::setEnabled);
}
}

32
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java
View File

@@ -1,32 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
/**
* Interface for PID Controller.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @deprecated All APIs which use this have been deprecated.
*/
@Deprecated(since = "2020", forRemoval = true)
@SuppressWarnings("SummaryJavadoc")
public interface PIDInterface {
void setPID(double p, double i, double d);
double getP();
double getI();
double getD();
void setSetpoint(double setpoint);
double getSetpoint();
double getError();
void reset();
}

23
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java
View File

@@ -1,23 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
/**
* This interface allows PIDController to write it's results to its output.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @deprecated Use DoubleConsumer and new PIDController class.
*/
@FunctionalInterface
@Deprecated(since = "2020", forRemoval = true)
public interface PIDOutput {
/**
* Set the output to the value calculated by PIDController.
*
* @param output the value calculated by PIDController
*/
void pidWrite(double output);
}

36
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
View File

@@ -1,36 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
/**
* This interface allows for PIDController to automatically read from this object.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @deprecated Use DoubleSupplier and new PIDController class.
*/
@Deprecated(since = "2020", forRemoval = true)
public interface PIDSource {
/**
* Set which parameter of the device you are using as a process control variable.
*
* @param pidSource An enum to select the parameter.
*/
void setPIDSourceType(PIDSourceType pidSource);
/**
* Get which parameter of the device you are using as a process control variable.
*
* @return the currently selected PID source parameter
*/
PIDSourceType getPIDSourceType();
/**
* Get the result to use in PIDController.
*
* @return the result to use in PIDController
*/
double pidGet();
}

16
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java
View File

@@ -1,16 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
/**
* A description for the type of output value to provide to a PIDController.
*
* <p>This class is provided by the OldCommands VendorDep
*/
@Deprecated(since = "2020", forRemoval = true)
public enum PIDSourceType {
kDisplacement,
kRate
}

69
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java
View File

@@ -1,69 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.buttons;
import edu.wpi.first.wpilibj.command.Command;
/**
* This class provides an easy way to link commands to OI inputs.
*
* <p>It is very easy to link a button to a command. For instance, you could link the trigger button
* of a joystick to a "score" command.
*
* <p>This class represents a subclass of Trigger that is specifically aimed at buttons on an
* operator interface as a common use case of the more generalized Trigger objects. This is a simple
* wrapper around Trigger with the method names renamed to fit the Button object use.
*
* <p>This class is provided by the OldCommands VendorDep
*/
public abstract class Button extends Trigger {
/**
* Starts the given command whenever the button is newly pressed.
*
* @param command the command to start
*/
public void whenPressed(final Command command) {
whenActive(command);
}
/**
* Constantly starts the given command while the button is held.
*
* <p>{@link Command#start()} will be called repeatedly while the button is held, and will be
* canceled when the button is released.
*
* @param command the command to start
*/
public void whileHeld(final Command command) {
whileActive(command);
}
/**
* Starts the command when the button is released.
*
* @param command the command to start
*/
public void whenReleased(final Command command) {
whenInactive(command);
}
/**
* Toggles the command whenever the button is pressed (on then off then on).
*
* @param command the command to start
*/
public void toggleWhenPressed(final Command command) {
toggleWhenActive(command);
}
/**
* Cancel the command when the button is pressed.
*
* @param command the command to start
*/
public void cancelWhenPressed(final Command command) {
cancelWhenActive(command);
}
}

44
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java
View File

@@ -1,44 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.buttons;
/**
* This {@link Button} is intended to be used within a program. The programmer can manually set its
* value. * Also includes a setting for whether or not it should invert its value.
*
* <p>This class is provided by the OldCommands VendorDep
*/
public class InternalButton extends Button {
private boolean m_pressed;
private boolean m_inverted;
/** Creates an InternalButton that is not inverted. */
public InternalButton() {
this(false);
}
/**
* Creates an InternalButton which is inverted depending on the input.
*
* @param inverted if false, then this button is pressed when set to true, otherwise it is pressed
* when set to false.
*/
public InternalButton(boolean inverted) {
m_pressed = m_inverted = inverted;
}
public void setInverted(boolean inverted) {
m_inverted = inverted;
}
public void setPressed(boolean pressed) {
m_pressed = pressed;
}
@Override
public boolean get() {
return m_pressed ^ m_inverted;
}
}

38
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java
View File

@@ -1,38 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.buttons;
import edu.wpi.first.wpilibj.GenericHID;
/**
* A {@link Button} that gets its state from a {@link GenericHID}.
*
* <p>This class is provided by the OldCommands VendorDep
*/
public class JoystickButton extends Button {
private final GenericHID m_joystick;
private final int m_buttonNumber;
/**
* Create a joystick button for triggering commands.
*
* @param joystick The GenericHID object that has the button (e.g. Joystick, KinectStick, etc)
* @param buttonNumber The button number (see {@link GenericHID#getRawButton(int) }
*/
public JoystickButton(GenericHID joystick, int buttonNumber) {
m_joystick = joystick;
m_buttonNumber = buttonNumber;
}
/**
* Gets the value of the joystick button.
*
* @return The value of the joystick button
*/
@Override
public boolean get() {
return m_joystick.getRawButton(m_buttonNumber);
}
}

31
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java
View File

@@ -1,31 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.buttons;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
/**
* A {@link Button} that uses a {@link NetworkTable} boolean field.
*
* <p>This class is provided by the OldCommands VendorDep
*/
public class NetworkButton extends Button {
private final NetworkTableEntry m_entry;
public NetworkButton(String table, String field) {
this(NetworkTableInstance.getDefault().getTable(table), field);
}
public NetworkButton(NetworkTable table, String field) {
m_entry = table.getEntry(field);
}
@Override
public boolean get() {
return m_entry.getInstance().isConnected() && m_entry.getBoolean(false);
}
}

51
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java
View File

@@ -1,51 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.buttons;
import edu.wpi.first.wpilibj.GenericHID;
/**
* A {@link Button} that gets its state from a POV on a {@link GenericHID}.
*
* <p>This class is provided by the OldCommands VendorDep
*/
public class POVButton extends Button {
private final GenericHID m_joystick;
private final int m_angle;
private final int m_povNumber;
/**
* Creates a POV button for triggering commands.
*
* @param joystick The GenericHID object that has the POV
* @param angle The desired angle in degrees (e.g. 90, 270)
* @param povNumber The POV number (see {@link GenericHID#getPOV(int)})
*/
public POVButton(GenericHID joystick, int angle, int povNumber) {
m_joystick = joystick;
m_angle = angle;
m_povNumber = povNumber;
}
/**
* Creates a POV button for triggering commands. By default, acts on POV 0
*
* @param joystick The GenericHID object that has the POV
* @param angle The desired angle (e.g. 90, 270)
*/
public POVButton(GenericHID joystick, int angle) {
this(joystick, angle, 0);
}
/**
* Checks whether the current value of the POV is the target angle.
*
* @return Whether the value of the POV matches the target angle
*/
@Override
public boolean get() {
return m_joystick.getPOV(m_povNumber) == m_angle;
}
}

183
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java
View File

@@ -1,183 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.buttons;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
/**
* This class provides an easy way to link commands to inputs.
*
* <p>It is very easy to link a button to a command. For instance, you could link the trigger button
* of a joystick to a "score" command.
*
* <p>It is encouraged that teams write a subclass of Trigger if they want to have something unusual
* (for instance, if they want to react to the user holding a button while the robot is reading a
* certain sensor input). For this, they only have to write the {@link Trigger#get()} method to get
* the full functionality of the Trigger class.
*
* <p>This class is provided by the OldCommands VendorDep
*/
public abstract class Trigger implements Sendable {
private volatile boolean m_sendablePressed;
/**
* Returns whether or not the trigger is active.
*
* <p>This method will be called repeatedly a command is linked to the Trigger.
*
* @return whether or not the trigger condition is active.
*/
public abstract boolean get();
/**
* Returns whether get() return true or the internal table for SmartDashboard use is pressed.
*
* @return whether get() return true or the internal table for SmartDashboard use is pressed.
*/
private boolean grab() {
return get() || m_sendablePressed;
}
/**
* Starts the given command whenever the trigger just becomes active.
*
* @param command the command to start
*/
public void whenActive(final Command command) {
new ButtonScheduler() {
private boolean m_pressedLast = grab();
@Override
public void execute() {
boolean pressed = grab();
if (!m_pressedLast && pressed) {
command.start();
}
m_pressedLast = pressed;
}
}.start();
}
/**
* Constantly starts the given command while the button is held.
*
* <p>{@link Command#start()} will be called repeatedly while the trigger is active, and will be
* canceled when the trigger becomes inactive.
*
* @param command the command to start
*/
public void whileActive(final Command command) {
new ButtonScheduler() {
private boolean m_pressedLast = grab();
@Override
public void execute() {
boolean pressed = grab();
if (pressed) {
command.start();
} else if (m_pressedLast && !pressed) {
command.cancel();
}
m_pressedLast = pressed;
}
}.start();
}
/**
* Starts the command when the trigger becomes inactive.
*
* @param command the command to start
*/
public void whenInactive(final Command command) {
new ButtonScheduler() {
private boolean m_pressedLast = grab();
@Override
public void execute() {
boolean pressed = grab();
if (m_pressedLast && !pressed) {
command.start();
}
m_pressedLast = pressed;
}
}.start();
}
/**
* Toggles a command when the trigger becomes active.
*
* @param command the command to toggle
*/
public void toggleWhenActive(final Command command) {
new ButtonScheduler() {
private boolean m_pressedLast = grab();
@Override
public void execute() {
boolean pressed = grab();
if (!m_pressedLast && pressed) {
if (command.isRunning()) {
command.cancel();
} else {
command.start();
}
}
m_pressedLast = pressed;
}
}.start();
}
/**
* Cancels a command when the trigger becomes active.
*
* @param command the command to cancel
*/
public void cancelWhenActive(final Command command) {
new ButtonScheduler() {
private boolean m_pressedLast = grab();
@Override
public void execute() {
boolean pressed = grab();
if (!m_pressedLast && pressed) {
command.cancel();
}
m_pressedLast = pressed;
}
}.start();
}
/**
* An internal class of {@link Trigger}. The user should ignore this, it is only public to
* interface between packages.
*/
public abstract static class ButtonScheduler {
public abstract void execute();
public void start() {
Scheduler.getInstance().addButton(this);
}
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Button");
builder.setSafeState(() -> m_sendablePressed = false);
builder.addBooleanProperty("pressed", this::grab, value -> m_sendablePressed = value);
}
}

627
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java
View File

@@ -1,627 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.RobotState;
import edu.wpi.first.wpilibj.Timer;
import java.util.Enumeration;
/**
* The Command class is at the very core of the entire command framework. Every command can be
* started with a call to {@link Command#start() start()}. Once a command is started it will call
* {@link Command#initialize() initialize()}, and then will repeatedly call {@link Command#execute()
* execute()} until the {@link Command#isFinished() isFinished()} returns true. Once it does, {@link
* Command#end() end()} will be called.
*
* <p>However, if at any point while it is running {@link Command#cancel() cancel()} is called, then
* the command will be stopped and {@link Command#interrupted() interrupted()} will be called.
*
* <p>If a command uses a {@link Subsystem}, then it should specify that it does so by calling the
* {@link Command#requires(Subsystem) requires(...)} method in its constructor. Note that a Command
* may have multiple requirements, and {@link Command#requires(Subsystem) requires(...)} should be
* called for each one.
*
* <p>If a command is running and a new command with shared requirements is started, then one of two
* things will happen. If the active command is interruptible, then {@link Command#cancel()
* cancel()} will be called and the command will be removed to make way for the new one. If the
* active command is not interruptible, the other one will not even be started, and the active one
* will continue functioning.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @see Subsystem
* @see CommandGroup
* @see IllegalUseOfCommandException
*/
public abstract class Command implements Sendable, AutoCloseable {
/** The time since this command was initialized. */
private double m_startTime = -1;
/** The time (in seconds) before this command "times out" (or -1 if no timeout). */
private double m_timeout = -1;
/** Whether or not this command has been initialized. */
private boolean m_initialized;
/** The required subsystems. */
private final Set m_requirements = new Set();
/** Whether or not it is running. */
private boolean m_running;
/** Whether or not it is interruptible. */
private boolean m_interruptible = true;
/** Whether or not it has been canceled. */
private boolean m_canceled;
/** Whether or not it has been locked. */
private boolean m_locked;
/** Whether this command should run when the robot is disabled. */
private boolean m_runWhenDisabled;
/** Whether or not this command has completed running. */
private boolean m_completed;
/** The {@link CommandGroup} this is in. */
private CommandGroup m_parent;
/** Creates a new command. The name of this command will be set to its class name. */
public Command() {
String name = getClass().getName();
SendableRegistry.add(this, name.substring(name.lastIndexOf('.') + 1));
}
/**
* Creates a new command with the given name.
*
* @param name the name for this command
* @throws IllegalArgumentException if name is null
*/
public Command(String name) {
if (name == null) {
throw new IllegalArgumentException("Name must not be null.");
}
SendableRegistry.add(this, name);
}
/**
* Creates a new command with the given timeout and a default name. The default name is the name
* of the class.
*
* @param timeout the time (in seconds) before this command "times out"
* @throws IllegalArgumentException if given a negative timeout
* @see Command#isTimedOut() isTimedOut()
*/
public Command(double timeout) {
this();
if (timeout < 0) {
throw new IllegalArgumentException("Timeout must not be negative. Given:" + timeout);
}
m_timeout = timeout;
}
/**
* Creates a new command with the given timeout and a default name. The default name is the name
* of the class.
*
* @param subsystem the subsystem that this command requires
* @throws IllegalArgumentException if given a negative timeout
* @see Command#isTimedOut() isTimedOut()
*/
public Command(Subsystem subsystem) {
this();
requires(subsystem);
}
/**
* Creates a new command with the given name.
*
* @param name the name for this command
* @param subsystem the subsystem that this command requires
* @throws IllegalArgumentException if name is null
*/
public Command(String name, Subsystem subsystem) {
this(name);
requires(subsystem);
}
/**
* Creates a new command with the given timeout and a default name. The default name is the name
* of the class.
*
* @param timeout the time (in seconds) before this command "times out"
* @param subsystem the subsystem that this command requires
* @throws IllegalArgumentException if given a negative timeout
* @see Command#isTimedOut() isTimedOut()
*/
public Command(double timeout, Subsystem subsystem) {
this(timeout);
requires(subsystem);
}
/**
* Creates a new command with the given name and timeout.
*
* @param name the name of the command
* @param timeout the time (in seconds) before this command "times out"
* @throws IllegalArgumentException if given a negative timeout or name was null.
* @see Command#isTimedOut() isTimedOut()
*/
public Command(String name, double timeout) {
this(name);
if (timeout < 0) {
throw new IllegalArgumentException("Timeout must not be negative. Given:" + timeout);
}
m_timeout = timeout;
}
/**
* Creates a new command with the given name and timeout.
*
* @param name the name of the command
* @param timeout the time (in seconds) before this command "times out"
* @param subsystem the subsystem that this command requires
* @throws IllegalArgumentException if given a negative timeout
* @throws IllegalArgumentException if given a negative timeout or name was null.
* @see Command#isTimedOut() isTimedOut()
*/
public Command(String name, double timeout, Subsystem subsystem) {
this(name, timeout);
requires(subsystem);
}
@Override
public void close() {
SendableRegistry.remove(this);
}
/**
* Sets the timeout of this command.
*
* @param seconds the timeout (in seconds)
* @throws IllegalArgumentException if seconds is negative
* @see Command#isTimedOut() isTimedOut()
*/
protected final synchronized void setTimeout(double seconds) {
if (seconds < 0) {
throw new IllegalArgumentException("Seconds must be positive. Given:" + seconds);
}
m_timeout = seconds;
}
/**
* Returns the time since this command was initialized (in seconds). This function will work even
* if there is no specified timeout.
*
* @return the time since this command was initialized (in seconds).
*/
public final synchronized double timeSinceInitialized() {
return m_startTime < 0 ? 0 : Timer.getFPGATimestamp() - m_startTime;
}
/**
* This method specifies that the given {@link Subsystem} is used by this command. This method is
* crucial to the functioning of the Command System in general.
*
* <p>Note that the recommended way to call this method is in the constructor.
*
* @param subsystem the {@link Subsystem} required
* @throws IllegalArgumentException if subsystem is null
* @throws IllegalUseOfCommandException if this command has started before or if it has been given
* to a {@link CommandGroup}
* @see Subsystem
*/
protected synchronized void requires(Subsystem subsystem) {
validate("Can not add new requirement to command");
if (subsystem != null) {
m_requirements.add(subsystem);
} else {
throw new IllegalArgumentException("Subsystem must not be null.");
}
}
/**
* Called when the command has been removed. This will call {@link Command#interrupted()
* interrupted()} or {@link Command#end() end()}.
*/
synchronized void removed() {
if (m_initialized) {
if (isCanceled()) {
interrupted();
_interrupted();
} else {
end();
_end();
}
}
m_initialized = false;
m_canceled = false;
m_running = false;
m_completed = true;
}
/**
* The run method is used internally to actually run the commands.
*
* @return whether or not the command should stay within the {@link Scheduler}.
*/
synchronized boolean run() {
if (!m_runWhenDisabled && m_parent == null && RobotState.isDisabled()) {
cancel();
}
if (isCanceled()) {
return false;
}
if (!m_initialized) {
m_initialized = true;
startTiming();
_initialize();
initialize();
}
_execute();
execute();
return !isFinished();
}
/** The initialize method is called the first time this Command is run after being started. */
protected void initialize() {}
/** A shadow method called before {@link Command#initialize() initialize()}. */
@SuppressWarnings("MethodName")
void _initialize() {}
/** The execute method is called repeatedly until this Command either finishes or is canceled. */
@SuppressWarnings("MethodName")
protected void execute() {}
/** A shadow method called before {@link Command#execute() execute()}. */
@SuppressWarnings("MethodName")
void _execute() {}
/**
* Returns whether this command is finished. If it is, then the command will be removed and {@link
* Command#end() end()} will be called.
*
* <p>It may be useful for a team to reference the {@link Command#isTimedOut() isTimedOut()}
* method for time-sensitive commands.
*
* <p>Returning false will result in the command never ending automatically. It may still be
* canceled manually or interrupted by another command. Returning true will result in the command
* executing once and finishing immediately. We recommend using {@link InstantCommand} for this.
*
* @return whether this command is finished.
* @see Command#isTimedOut() isTimedOut()
*/
protected abstract boolean isFinished();
/**
* Called when the command ended peacefully. This is where you may want to wrap up loose ends,
* like shutting off a motor that was being used in the command.
*/
protected void end() {}
/** A shadow method called after {@link Command#end() end()}. */
@SuppressWarnings("MethodName")
void _end() {}
/**
* Called when the command ends because somebody called {@link Command#cancel() cancel()} or
* another command shared the same requirements as this one, and booted it out.
*
* <p>This is where you may want to wrap up loose ends, like shutting off a motor that was being
* used in the command.
*
* <p>Generally, it is useful to simply call the {@link Command#end() end()} method within this
* method, as done here.
*/
protected void interrupted() {
end();
}
/** A shadow method called after {@link Command#interrupted() interrupted()}. */
@SuppressWarnings("MethodName")
void _interrupted() {}
/**
* Called to indicate that the timer should start. This is called right before {@link
* Command#initialize() initialize()} is, inside the {@link Command#run() run()} method.
*/
private void startTiming() {
m_startTime = Timer.getFPGATimestamp();
}
/**
* Returns whether or not the {@link Command#timeSinceInitialized() timeSinceInitialized()} method
* returns a number which is greater than or equal to the timeout for the command. If there is no
* timeout, this will always return false.
*
* @return whether the time has expired
*/
protected synchronized boolean isTimedOut() {
return m_timeout != -1 && timeSinceInitialized() >= m_timeout;
}
/**
* Returns the requirements (as an {@link Enumeration Enumeration} of {@link Subsystem
* Subsystems}) of this command.
*
* @return the requirements (as an {@link Enumeration Enumeration} of {@link Subsystem
* Subsystems}) of this command
*/
synchronized Enumeration<?> getRequirements() {
return m_requirements.getElements();
}
/** Prevents further changes from being made. */
synchronized void lockChanges() {
m_locked = true;
}
/**
* If changes are locked, then this will throw an {@link IllegalUseOfCommandException}.
*
* @param message the message to say (it is appended by a default message)
*/
synchronized void validate(String message) {
if (m_locked) {
throw new IllegalUseOfCommandException(
message + " after being started or being added to a command group");
}
}
/**
* Sets the parent of this command. No actual change is made to the group.
*
* @param parent the parent
* @throws IllegalUseOfCommandException if this {@link Command} already is already in a group
*/
synchronized void setParent(CommandGroup parent) {
if (m_parent != null) {
throw new IllegalUseOfCommandException(
"Can not give command to a command group after already being put in a command group");
}
lockChanges();
m_parent = parent;
}
/**
* Returns whether the command has a parent.
*
* @return true if the command has a parent.
*/
synchronized boolean isParented() {
return m_parent != null;
}
/**
* Clears list of subsystem requirements. This is only used by {@link ConditionalCommand} so
* canceling the chosen command works properly in {@link CommandGroup}.
*/
protected void clearRequirements() {
m_requirements.clear();
}
/**
* Starts up the command. Gets the command ready to start.
*
* <p>Note that the command will eventually start, however it will not necessarily do so
* immediately, and may in fact be canceled before initialize is even called.
*
* @throws IllegalUseOfCommandException if the command is a part of a CommandGroup
*/
public synchronized void start() {
lockChanges();
if (m_parent != null) {
throw new IllegalUseOfCommandException(
"Can not start a command that is a part of a command group");
}
Scheduler.getInstance().add(this);
m_completed = false;
}
/**
* This is used internally to mark that the command has been started. The lifecycle of a command
* is:
*
* <p>startRunning() is called. run() is called (multiple times potentially) removed() is called.
*
* <p>It is very important that startRunning and removed be called in order or some assumptions of
* the code will be broken.
*/
synchronized void startRunning() {
m_running = true;
m_startTime = -1;
}
/**
* Returns whether or not the command is running. This may return true even if the command has
* just been canceled, as it may not have yet called {@link Command#interrupted()}.
*
* @return whether or not the command is running
*/
public synchronized boolean isRunning() {
return m_running;
}
/**
* This will cancel the current command.
*
* <p>This will cancel the current command eventually. It can be called multiple times. And it can
* be called when the command is not running. If the command is running though, then the command
* will be marked as canceled and eventually removed.
*
* <p>A command can not be canceled if it is a part of a command group, you must cancel the
* command group instead.
*
* @throws IllegalUseOfCommandException if this command is a part of a command group
*/
public synchronized void cancel() {
if (m_parent != null) {
throw new IllegalUseOfCommandException(
"Can not manually cancel a command in a command " + "group");
}
_cancel();
}
/**
* This works like cancel(), except that it doesn't throw an exception if it is a part of a
* command group. Should only be called by the parent command group.
*/
@SuppressWarnings("MethodName")
synchronized void _cancel() {
if (isRunning()) {
m_canceled = true;
}
}
/**
* Returns whether or not this has been canceled.
*
* @return whether or not this has been canceled
*/
public synchronized boolean isCanceled() {
return m_canceled;
}
/**
* Whether or not this command has completed running.
*
* @return whether or not this command has completed running.
*/
public synchronized boolean isCompleted() {
return m_completed;
}
/**
* Returns whether or not this command can be interrupted.
*
* @return whether or not this command can be interrupted
*/
public synchronized boolean isInterruptible() {
return m_interruptible;
}
/**
* Sets whether or not this command can be interrupted.
*
* @param interruptible whether or not this command can be interrupted
*/
protected synchronized void setInterruptible(boolean interruptible) {
m_interruptible = interruptible;
}
/**
* Checks if the command requires the given {@link Subsystem}.
*
* @param system the system
* @return whether or not the subsystem is required, or false if given null
*/
public synchronized boolean doesRequire(Subsystem system) {
return m_requirements.contains(system);
}
/**
* Returns the {@link CommandGroup} that this command is a part of. Will return null if this
* {@link Command} is not in a group.
*
* @return the {@link CommandGroup} that this command is a part of (or null if not in group)
*/
public synchronized CommandGroup getGroup() {
return m_parent;
}
/**
* Sets whether or not this {@link Command} should run when the robot is disabled.
*
* <p>By default a command will not run when the robot is disabled, and will in fact be canceled.
*
* @param run whether or not this command should run when the robot is disabled
*/
public void setRunWhenDisabled(boolean run) {
m_runWhenDisabled = run;
}
/**
* Returns whether or not this {@link Command} will run when the robot is disabled, or if it will
* cancel itself.
*
* @return True if this command will run when the robot is disabled.
*/
public boolean willRunWhenDisabled() {
return m_runWhenDisabled;
}
/**
* Gets the name of this Command.
*
* @return Name
*/
public String getName() {
return SendableRegistry.getName(this);
}
/**
* Sets the name of this Command.
*
* @param name name
*/
public void setName(String name) {
SendableRegistry.setName(this, name);
}
/**
* Gets the subsystem name of this Command.
*
* @return Subsystem name
*/
public String getSubsystem() {
return SendableRegistry.getSubsystem(this);
}
/**
* Sets the subsystem name of this Command.
*
* @param subsystem subsystem name
*/
public void setSubsystem(String subsystem) {
SendableRegistry.setSubsystem(this, subsystem);
}
/**
* The string representation for a {@link Command} is by default its name.
*
* @return the string representation of this object
*/
@Override
public String toString() {
return getName();
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Command");
builder.addStringProperty(".name", this::getName, null);
builder.addBooleanProperty(
"running",
this::isRunning,
value -> {
if (value) {
if (!isRunning()) {
start();
}
} else {
if (isRunning()) {
cancel();
}
}
});
builder.addBooleanProperty(".isParented", this::isParented, null);
}
}

410
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
View File

@@ -1,410 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
import static java.util.Objects.requireNonNull;
import java.util.Enumeration;
import java.util.Vector;
/**
* A {@link CommandGroup} is a list of commands which are executed in sequence.
*
* <p>Commands in a {@link CommandGroup} are added using the {@link
* CommandGroup#addSequential(Command) addSequential(...)} method and are called sequentially.
* {@link CommandGroup CommandGroups} are themselves {@link Command commands} and can be given to
* other {@link CommandGroup CommandGroups}.
*
* <p>{@link CommandGroup CommandGroups} will carry all of the requirements of their {@link Command
* subcommands}. Additional requirements can be specified by calling {@link
* CommandGroup#requires(Subsystem) requires(...)} normally in the constructor.
*
* <p>CommandGroups can also execute commands in parallel, simply by adding them using {@link
* CommandGroup#addParallel(Command) addParallel(...)}.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @see Command
* @see Subsystem
* @see IllegalUseOfCommandException
*/
public class CommandGroup extends Command {
/** The commands in this group (stored in entries). */
@SuppressWarnings("PMD.UseArrayListInsteadOfVector")
private final Vector<Entry> m_commands = new Vector<>();
/*
* Intentionally package private
*/
/** The active children in this group (stored in entries). */
@SuppressWarnings("PMD.UseArrayListInsteadOfVector")
final Vector<Entry> m_children = new Vector<>();
/** The current command, -1 signifies that none have been run. */
private int m_currentCommandIndex = -1;
/**
* Creates a new {@link CommandGroup CommandGroup}. The name of this command will be set to its
* class name.
*/
public CommandGroup() {}
/**
* Creates a new {@link CommandGroup CommandGroup} with the given name.
*
* @param name the name for this command group
* @throws IllegalArgumentException if name is null
*/
public CommandGroup(String name) {
super(name);
}
/**
* Adds a new {@link Command Command} to the group. The {@link Command Command} will be started
* after all the previously added {@link Command Commands}.
*
* <p>Note that any requirements the given {@link Command Command} has will be added to the group.
* For this reason, a {@link Command Command's} requirements can not be changed after being added
* to a group.
*
* <p>It is recommended that this method be called in the constructor.
*
* @param command The {@link Command Command} to be added
* @throws IllegalUseOfCommandException if the group has been started before or been given to
* another group
* @throws IllegalArgumentException if command is null
*/
public final synchronized void addSequential(Command command) {
validate("Can not add new command to command group");
if (command == null) {
throw new IllegalArgumentException("Given null command");
}
command.setParent(this);
m_commands.addElement(new Entry(command, Entry.IN_SEQUENCE));
for (Enumeration<?> e = command.getRequirements(); e.hasMoreElements(); ) {
requires((Subsystem) e.nextElement());
}
}
/**
* Adds a new {@link Command Command} to the group with a given timeout. The {@link Command
* Command} will be started after all the previously added commands.
*
* <p>Once the {@link Command Command} is started, it will be run until it finishes or the time
* expires, whichever is sooner. Note that the given {@link Command Command} will have no
* knowledge that it is on a timer.
*
* <p>Note that any requirements the given {@link Command Command} has will be added to the group.
* For this reason, a {@link Command Command's} requirements can not be changed after being added
* to a group.
*
* <p>It is recommended that this method be called in the constructor.
*
* @param command The {@link Command Command} to be added
* @param timeout The timeout (in seconds)
* @throws IllegalUseOfCommandException if the group has been started before or been given to
* another group or if the {@link Command Command} has been started before or been given to
* another group
* @throws IllegalArgumentException if command is null or timeout is negative
*/
public final synchronized void addSequential(Command command, double timeout) {
validate("Can not add new command to command group");
if (command == null) {
throw new IllegalArgumentException("Given null command");
}
if (timeout < 0) {
throw new IllegalArgumentException("Can not be given a negative timeout");
}
command.setParent(this);
m_commands.addElement(new Entry(command, Entry.IN_SEQUENCE, timeout));
for (Enumeration<?> e = command.getRequirements(); e.hasMoreElements(); ) {
requires((Subsystem) e.nextElement());
}
}
/**
* Adds a new child {@link Command} to the group. The {@link Command} will be started after all
* the previously added {@link Command Commands}.
*
* <p>Instead of waiting for the child to finish, a {@link CommandGroup} will have it run at the
* same time as the subsequent {@link Command Commands}. The child will run until either it
* finishes, a new child with conflicting requirements is started, or the main sequence runs a
* {@link Command} with conflicting requirements. In the latter two cases, the child will be
* canceled even if it says it can't be interrupted.
*
* <p>Note that any requirements the given {@link Command Command} has will be added to the group.
* For this reason, a {@link Command Command's} requirements can not be changed after being added
* to a group.
*
* <p>It is recommended that this method be called in the constructor.
*
* @param command The command to be added
* @throws IllegalUseOfCommandException if the group has been started before or been given to
* another command group
* @throws IllegalArgumentException if command is null
*/
public final synchronized void addParallel(Command command) {
requireNonNull(command, "Provided command was null");
validate("Can not add new command to command group");
command.setParent(this);
m_commands.addElement(new Entry(command, Entry.BRANCH_CHILD));
for (Enumeration<?> e = command.getRequirements(); e.hasMoreElements(); ) {
requires((Subsystem) e.nextElement());
}
}
/**
* Adds a new child {@link Command} to the group with the given timeout. The {@link Command} will
* be started after all the previously added {@link Command Commands}.
*
* <p>Once the {@link Command Command} is started, it will run until it finishes, is interrupted,
* or the time expires, whichever is sooner. Note that the given {@link Command Command} will have
* no knowledge that it is on a timer.
*
* <p>Instead of waiting for the child to finish, a {@link CommandGroup} will have it run at the
* same time as the subsequent {@link Command Commands}. The child will run until either it
* finishes, the timeout expires, a new child with conflicting requirements is started, or the
* main sequence runs a {@link Command} with conflicting requirements. In the latter two cases,
* the child will be canceled even if it says it can't be interrupted.
*
* <p>Note that any requirements the given {@link Command Command} has will be added to the group.
* For this reason, a {@link Command Command's} requirements can not be changed after being added
* to a group.
*
* <p>It is recommended that this method be called in the constructor.
*
* @param command The command to be added
* @param timeout The timeout (in seconds)
* @throws IllegalUseOfCommandException if the group has been started before or been given to
* another command group
* @throws IllegalArgumentException if command is null
*/
public final synchronized void addParallel(Command command, double timeout) {
requireNonNull(command, "Provided command was null");
if (timeout < 0) {
throw new IllegalArgumentException("Can not be given a negative timeout");
}
validate("Can not add new command to command group");
command.setParent(this);
m_commands.addElement(new Entry(command, Entry.BRANCH_CHILD, timeout));
for (Enumeration<?> e = command.getRequirements(); e.hasMoreElements(); ) {
requires((Subsystem) e.nextElement());
}
}
@Override
@SuppressWarnings("MethodName")
void _initialize() {
m_currentCommandIndex = -1;
}
@Override
@SuppressWarnings({"MethodName", "PMD.AvoidReassigningLoopVariables"})
void _execute() {
Entry entry = null;
Command cmd = null;
boolean firstRun = false;
if (m_currentCommandIndex == -1) {
firstRun = true;
m_currentCommandIndex = 0;
}
while (m_currentCommandIndex < m_commands.size()) {
if (cmd != null) {
if (entry.isTimedOut()) {
cmd._cancel();
}
if (cmd.run()) {
break;
} else {
cmd.removed();
m_currentCommandIndex++;
firstRun = true;
cmd = null;
continue;
}
}
entry = m_commands.elementAt(m_currentCommandIndex);
cmd = null;
switch (entry.m_state) {
case Entry.IN_SEQUENCE:
cmd = entry.m_command;
if (firstRun) {
cmd.startRunning();
cancelConflicts(cmd);
}
firstRun = false;
break;
case Entry.BRANCH_PEER:
m_currentCommandIndex++;
entry.m_command.start();
break;
case Entry.BRANCH_CHILD:
m_currentCommandIndex++;
cancelConflicts(entry.m_command);
entry.m_command.startRunning();
m_children.addElement(entry);
break;
default:
break;
}
}
// Run Children
for (int i = 0; i < m_children.size(); i++) {
entry = m_children.elementAt(i);
Command child = entry.m_command;
if (entry.isTimedOut()) {
child._cancel();
}
if (!child.run()) {
child.removed();
m_children.removeElementAt(i--);
}
}
}
@Override
@SuppressWarnings("MethodName")
void _end() {
// Theoretically, we don't have to check this, but we do if teams override
// the isFinished method
if (m_currentCommandIndex != -1 && m_currentCommandIndex < m_commands.size()) {
Command cmd = m_commands.elementAt(m_currentCommandIndex).m_command;
cmd._cancel();
cmd.removed();
}
Enumeration<?> children = m_children.elements();
while (children.hasMoreElements()) {
Command cmd = ((Entry) children.nextElement()).m_command;
cmd._cancel();
cmd.removed();
}
m_children.removeAllElements();
}
@Override
@SuppressWarnings("MethodName")
void _interrupted() {
_end();
}
/**
* Returns true if all the {@link Command Commands} in this group have been started and have
* finished.
*
* <p>Teams may override this method, although they should probably reference super.isFinished()
* if they do.
*
* @return whether this {@link CommandGroup} is finished
*/
@Override
protected boolean isFinished() {
return m_currentCommandIndex >= m_commands.size() && m_children.isEmpty();
}
// Can be overwritten by teams
@Override
protected void initialize() {}
// Can be overwritten by teams
@Override
protected void execute() {}
// Can be overwritten by teams
@Override
protected void end() {}
// Can be overwritten by teams
@Override
protected void interrupted() {}
/**
* Returns whether or not this group is interruptible. A command group will be uninterruptible if
* {@link CommandGroup#setInterruptible(boolean) setInterruptible(false)} was called or if it is
* currently running an uninterruptible command or child.
*
* @return whether or not this {@link CommandGroup} is interruptible.
*/
@Override
public synchronized boolean isInterruptible() {
if (!super.isInterruptible()) {
return false;
}
if (m_currentCommandIndex != -1 && m_currentCommandIndex < m_commands.size()) {
Command cmd = m_commands.elementAt(m_currentCommandIndex).m_command;
if (!cmd.isInterruptible()) {
return false;
}
}
for (int i = 0; i < m_children.size(); i++) {
if (!m_children.elementAt(i).m_command.isInterruptible()) {
return false;
}
}
return true;
}
@SuppressWarnings("PMD.AvoidReassigningLoopVariables")
private void cancelConflicts(Command command) {
for (int i = 0; i < m_children.size(); i++) {
Command child = m_children.elementAt(i).m_command;
Enumeration<?> requirements = command.getRequirements();
while (requirements.hasMoreElements()) {
Object requirement = requirements.nextElement();
if (child.doesRequire((Subsystem) requirement)) {
child._cancel();
child.removed();
m_children.removeElementAt(i--);
break;
}
}
}
}
private static class Entry {
private static final int IN_SEQUENCE = 0;
private static final int BRANCH_PEER = 1;
private static final int BRANCH_CHILD = 2;
private final Command m_command;
private final int m_state;
private final double m_timeout;
Entry(Command command, int state) {
m_command = command;
m_state = state;
m_timeout = -1;
}
Entry(Command command, int state, double timeout) {
m_command = command;
m_state = state;
m_timeout = timeout;
}
boolean isTimedOut() {
if (m_timeout == -1) {
return false;
} else {
double time = m_command.timeSinceInitialized();
return time != 0 && time >= m_timeout;
}
}
}
}

163
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java
View File

@@ -1,163 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
import java.util.Enumeration;
/**
* A {@link ConditionalCommand} is a {@link Command} that starts one of two commands.
*
* <p>A {@link ConditionalCommand} uses m_condition to determine whether it should run m_onTrue or
* m_onFalse.
*
* <p>A {@link ConditionalCommand} adds the proper {@link Command} to the {@link Scheduler} during
* {@link ConditionalCommand#initialize()} and then {@link ConditionalCommand#isFinished()} will
* return true once that {@link Command} has finished executing.
*
* <p>If no {@link Command} is specified for m_onFalse, the occurrence of that condition will be a
* no-op.
*
* <p>A ConditionalCommand will require the superset of subsystems of the onTrue and onFalse
* commands.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @see Command
* @see Scheduler
*/
public abstract class ConditionalCommand extends Command {
/** The Command to execute if {@link ConditionalCommand#condition()} returns true. */
private Command m_onTrue;
/** The Command to execute if {@link ConditionalCommand#condition()} returns false. */
private Command m_onFalse;
/** Stores command chosen by condition. */
private Command m_chosenCommand;
private void requireAll() {
if (m_onTrue != null) {
for (Enumeration<?> e = m_onTrue.getRequirements(); e.hasMoreElements(); ) {
requires((Subsystem) e.nextElement());
}
}
if (m_onFalse != null) {
for (Enumeration<?> e = m_onFalse.getRequirements(); e.hasMoreElements(); ) {
requires((Subsystem) e.nextElement());
}
}
}
/**
* Creates a new ConditionalCommand with given onTrue and onFalse Commands.
*
* <p>Users of this constructor should also override condition().
*
* @param onTrue The Command to execute if {@link ConditionalCommand#condition()} returns true
*/
public ConditionalCommand(Command onTrue) {
this(onTrue, null);
}
/**
* Creates a new ConditionalCommand with given onTrue and onFalse Commands.
*
* <p>Users of this constructor should also override condition().
*
* @param onTrue The Command to execute if {@link ConditionalCommand#condition()} returns true
* @param onFalse The Command to execute if {@link ConditionalCommand#condition()} returns false
*/
public ConditionalCommand(Command onTrue, Command onFalse) {
m_onTrue = onTrue;
m_onFalse = onFalse;
requireAll();
}
/**
* Creates a new ConditionalCommand with given name and onTrue and onFalse Commands.
*
* <p>Users of this constructor should also override condition().
*
* @param name the name for this command group
* @param onTrue The Command to execute if {@link ConditionalCommand#condition()} returns true
*/
public ConditionalCommand(String name, Command onTrue) {
this(name, onTrue, null);
}
/**
* Creates a new ConditionalCommand with given name and onTrue and onFalse Commands.
*
* <p>Users of this constructor should also override condition().
*
* @param name the name for this command group
* @param onTrue The Command to execute if {@link ConditionalCommand#condition()} returns true
* @param onFalse The Command to execute if {@link ConditionalCommand#condition()} returns false
*/
public ConditionalCommand(String name, Command onTrue, Command onFalse) {
super(name);
m_onTrue = onTrue;
m_onFalse = onFalse;
requireAll();
}
/**
* The Condition to test to determine which Command to run.
*
* @return true if m_onTrue should be run or false if m_onFalse should be run.
*/
protected abstract boolean condition();
/** Calls {@link ConditionalCommand#condition()} and runs the proper command. */
@Override
protected void _initialize() {
if (condition()) {
m_chosenCommand = m_onTrue;
} else {
m_chosenCommand = m_onFalse;
}
if (m_chosenCommand != null) {
/*
* This is a hack to make canceling the chosen command inside a
* CommandGroup work properly
*/
m_chosenCommand.clearRequirements();
m_chosenCommand.start();
}
super._initialize();
}
@Override
protected synchronized void _cancel() {
if (m_chosenCommand != null && m_chosenCommand.isRunning()) {
m_chosenCommand.cancel();
}
super._cancel();
}
@Override
protected boolean isFinished() {
if (m_chosenCommand != null) {
return m_chosenCommand.isCompleted();
} else {
return true;
}
}
@Override
protected void _interrupted() {
if (m_chosenCommand != null && m_chosenCommand.isRunning()) {
m_chosenCommand.cancel();
}
super._interrupted();
}
}

32
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
View File

@@ -1,32 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
/**
* This exception will be thrown if a command is used illegally. There are several ways for this to
* happen.
*
* <p>Basically, a command becomes "locked" after it is first started or added to a command group.
*
* <p>This exception should be thrown if (after a command has been locked) its requirements change,
* it is put into multiple command groups, it is started from outside its command group, or it adds
* a new child.
*
* <p>This class is provided by the OldCommands VendorDep
*/
@SuppressWarnings("serial")
public class IllegalUseOfCommandException extends RuntimeException {
/** Instantiates an {@link IllegalUseOfCommandException}. */
public IllegalUseOfCommandException() {}
/**
* Instantiates an {@link IllegalUseOfCommandException} with the given message.
*
* @param message the message
*/
public IllegalUseOfCommandException(String message) {
super(message);
}
}

108
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java
View File

@@ -1,108 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
/**
* This command will execute once, then finish immediately afterward.
*
* <p>Subclassing {@link InstantCommand} is shorthand for returning true from {@link Command
* isFinished}.
*
* <p>This class is provided by the OldCommands VendorDep
*/
public class InstantCommand extends Command {
private Runnable m_func;
public InstantCommand() {}
/**
* Creates a new {@link InstantCommand InstantCommand} with the given name.
*
* @param name the name for this command
*/
public InstantCommand(String name) {
super(name);
}
/**
* Creates a new {@link InstantCommand InstantCommand} with the given requirement.
*
* @param subsystem the subsystem this command requires
*/
public InstantCommand(Subsystem subsystem) {
super(subsystem);
}
/**
* Creates a new {@link InstantCommand InstantCommand} with the given name and requirement.
*
* @param name the name for this command
* @param subsystem the subsystem this command requires
*/
public InstantCommand(String name, Subsystem subsystem) {
super(name, subsystem);
}
/**
* Creates a new {@link InstantCommand InstantCommand}.
*
* @param func the function to run when {@link Command#initialize() initialize()} is run
*/
public InstantCommand(Runnable func) {
m_func = func;
}
/**
* Creates a new {@link InstantCommand InstantCommand}.
*
* @param name the name for this command
* @param func the function to run when {@link Command#initialize() initialize()} is run
*/
public InstantCommand(String name, Runnable func) {
super(name);
m_func = func;
}
/**
* Creates a new {@link InstantCommand InstantCommand}.
*
* @param requirement the subsystem this command requires
* @param func the function to run when {@link Command#initialize() initialize()} is run
*/
public InstantCommand(Subsystem requirement, Runnable func) {
super(requirement);
m_func = func;
}
/**
* Creates a new {@link InstantCommand InstantCommand}.
*
* @param name the name for this command
* @param requirement the subsystem this command requires
* @param func the function to run when {@link Command#initialize() initialize()} is run
*/
public InstantCommand(String name, Subsystem requirement, Runnable func) {
super(name, requirement);
m_func = func;
}
@Override
protected boolean isFinished() {
return true;
}
/**
* Trigger the stored function.
*
* <p>Called just before this Command runs the first time.
*/
@Override
protected void _initialize() {
super._initialize();
if (m_func != null) {
m_func.run();
}
}
}

62
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java
View File

@@ -1,62 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
/**
* An element that is in a LinkedList.
*
* <p>This class is provided by the OldCommands VendorDep
*/
class LinkedListElement {
private LinkedListElement m_next;
private LinkedListElement m_previous;
private Command m_data;
public void setData(Command newData) {
m_data = newData;
}
public Command getData() {
return m_data;
}
public LinkedListElement getNext() {
return m_next;
}
public LinkedListElement getPrevious() {
return m_previous;
}
public void add(LinkedListElement listElement) {
if (m_next == null) {
m_next = listElement;
m_next.m_previous = this;
} else {
m_next.m_previous = listElement;
listElement.m_next = m_next;
listElement.m_previous = this;
m_next = listElement;
}
}
@SuppressWarnings("PMD.EmptyIfStmt")
public LinkedListElement remove() {
if (m_previous == null && m_next == null) {
// no-op
} else if (m_next == null) {
m_previous.m_next = null;
} else if (m_previous == null) {
m_next.m_previous = null;
} else {
m_next.m_previous = m_previous;
m_previous.m_next = m_next;
}
LinkedListElement returnNext = m_next;
m_next = null;
m_previous = null;
return returnNext;
}
}

268
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
View File

@@ -1,268 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
/**
* This class defines a {@link Command} which interacts heavily with a PID loop.
*
* <p>It provides some convenience methods to run an internal {@link PIDController} . It will also
* start and stop said {@link PIDController} when the {@link PIDCommand} is first initialized and
* ended/interrupted.
*
* <p>This class is provided by the OldCommands VendorDep
*/
public abstract class PIDCommand extends Command {
/** The internal {@link PIDController}. */
private final PIDController m_controller;
/** An output which calls {@link PIDCommand#usePIDOutput(double)}. */
private final PIDOutput m_output = this::usePIDOutput;
/** A source which calls {@link PIDCommand#returnPIDInput()}. */
private final PIDSource m_source =
new PIDSource() {
@Override
public void setPIDSourceType(PIDSourceType pidSource) {}
@Override
public PIDSourceType getPIDSourceType() {
return PIDSourceType.kDisplacement;
}
@Override
public double pidGet() {
return returnPIDInput();
}
};
/**
* Instantiates a {@link PIDCommand} that will use the given p, i and d values.
*
* @param name the name of the command
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
*/
public PIDCommand(String name, double p, double i, double d) {
super(name);
m_controller = new PIDController(p, i, d, m_source, m_output);
}
/**
* Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will also space
* the time between PID loop calculations to be equal to the given period.
*
* @param name the name
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param period the time (in seconds) between calculations
*/
public PIDCommand(String name, double p, double i, double d, double period) {
super(name);
m_controller = new PIDController(p, i, d, m_source, m_output, period);
}
/**
* Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will use the
* class name as its name.
*
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
*/
public PIDCommand(double p, double i, double d) {
m_controller = new PIDController(p, i, d, m_source, m_output);
}
/**
* Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will use the
* class name as its name. It will also space the time between PID loop calculations to be equal
* to the given period.
*
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param period the time (in seconds) between calculations
*/
public PIDCommand(double p, double i, double d, double period) {
m_controller = new PIDController(p, i, d, m_source, m_output, period);
}
/**
* Instantiates a {@link PIDCommand} that will use the given p, i and d values.
*
* @param name the name of the command
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param subsystem the subsystem that this command requires
*/
public PIDCommand(String name, double p, double i, double d, Subsystem subsystem) {
super(name, subsystem);
m_controller = new PIDController(p, i, d, m_source, m_output);
}
/**
* Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will also space
* the time between PID loop calculations to be equal to the given period.
*
* @param name the name
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param period the time (in seconds) between calculations
* @param subsystem the subsystem that this command requires
*/
public PIDCommand(String name, double p, double i, double d, double period, Subsystem subsystem) {
super(name, subsystem);
m_controller = new PIDController(p, i, d, m_source, m_output, period);
}
/**
* Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will use the
* class name as its name.
*
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param subsystem the subsystem that this command requires
*/
public PIDCommand(double p, double i, double d, Subsystem subsystem) {
super(subsystem);
m_controller = new PIDController(p, i, d, m_source, m_output);
}
/**
* Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will use the
* class name as its name. It will also space the time between PID loop calculations to be equal
* to the given period.
*
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param period the time (in seconds) between calculations
* @param subsystem the subsystem that this command requires
*/
public PIDCommand(double p, double i, double d, double period, Subsystem subsystem) {
super(subsystem);
m_controller = new PIDController(p, i, d, m_source, m_output, period);
}
/**
* Returns the {@link PIDController} used by this {@link PIDCommand}. Use this if you would like
* to fine tune the pid loop.
*
* @return the {@link PIDController} used by this {@link PIDCommand}
*/
protected PIDController getPIDController() {
return m_controller;
}
@Override
@SuppressWarnings("MethodName")
void _initialize() {
m_controller.enable();
}
@Override
@SuppressWarnings("MethodName")
void _end() {
m_controller.disable();
}
@Override
@SuppressWarnings("MethodName")
void _interrupted() {
_end();
}
/**
* Adds the given value to the setpoint. If {@link PIDCommand#setInputRange(double, double)
* setInputRange(...)} was used, then the bounds will still be honored by this method.
*
* @param deltaSetpoint the change in the setpoint
*/
public void setSetpointRelative(double deltaSetpoint) {
setSetpoint(getSetpoint() + deltaSetpoint);
}
/**
* Sets the setpoint to the given value. If {@link PIDCommand#setInputRange(double, double)
* setInputRange(...)} was called, then the given setpoint will be trimmed to fit within the
* range.
*
* @param setpoint the new setpoint
*/
protected void setSetpoint(double setpoint) {
m_controller.setSetpoint(setpoint);
}
/**
* Returns the setpoint.
*
* @return the setpoint
*/
protected double getSetpoint() {
return m_controller.getSetpoint();
}
/**
* Returns the current position.
*
* @return the current position
*/
protected double getPosition() {
return returnPIDInput();
}
/**
* Sets the maximum and minimum values expected from the input and setpoint.
*
* @param minimumInput the minimum value expected from the input and setpoint
* @param maximumInput the maximum value expected from the input and setpoint
*/
protected void setInputRange(double minimumInput, double maximumInput) {
m_controller.setInputRange(minimumInput, maximumInput);
}
/**
* Returns the input for the pid loop.
*
* <p>It returns the input for the pid loop, so if this command was based off of a gyro, then it
* should return the angle of the gyro.
*
* <p>All subclasses of {@link PIDCommand} must override this method.
*
* <p>This method will be called in a different thread then the {@link Scheduler} thread.
*
* @return the value the pid loop should use as input
*/
protected abstract double returnPIDInput();
/**
* Uses the value that the pid loop calculated. The calculated value is the "output" parameter.
* This method is a good time to set motor values, maybe something along the lines of <code>
* driveline.tankDrive(output, -output)</code>
*
* <p>All subclasses of {@link PIDCommand} must override this method.
*
* <p>This method will be called in a different thread then the {@link Scheduler} thread.
*
* @param output the value the pid loop calculated
*/
protected abstract void usePIDOutput(double output);
@Override
public void initSendable(SendableBuilder builder) {
m_controller.initSendable(builder);
super.initSendable(builder);
builder.setSmartDashboardType("PIDCommand");
}
}

266
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
View File

@@ -1,266 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
/**
* This class is designed to handle the case where there is a {@link Subsystem} which uses a single
* {@link PIDController} almost constantly (for instance, an elevator which attempts to stay at a
* constant height).
*
* <p>It provides some convenience methods to run an internal {@link PIDController} . It also allows
* access to the internal {@link PIDController} in order to give total control to the programmer.
*
* <p>This class is provided by the OldCommands VendorDep
*/
public abstract class PIDSubsystem extends Subsystem {
/** The internal {@link PIDController}. */
private final PIDController m_controller;
/** An output which calls {@link PIDCommand#usePIDOutput(double)}. */
private final PIDOutput m_output = this::usePIDOutput;
/** A source which calls {@link PIDCommand#returnPIDInput()}. */
private final PIDSource m_source =
new PIDSource() {
@Override
public void setPIDSourceType(PIDSourceType pidSource) {}
@Override
public PIDSourceType getPIDSourceType() {
return PIDSourceType.kDisplacement;
}
@Override
public double pidGet() {
return returnPIDInput();
}
};
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
*
* @param name the name
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
*/
public PIDSubsystem(String name, double p, double i, double d) {
super(name);
m_controller = new PIDController(p, i, d, m_source, m_output);
addChild("PIDController", m_controller);
}
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i, d, and f values.
*
* @param name the name
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param f the feed forward value
*/
public PIDSubsystem(String name, double p, double i, double d, double f) {
super(name);
m_controller = new PIDController(p, i, d, f, m_source, m_output);
addChild("PIDController", m_controller);
}
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i, d, and f values. It will also
* space the time between PID loop calculations to be equal to the given period.
*
* @param name the name
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param f the feed forward value
* @param period the time (in seconds) between calculations
*/
public PIDSubsystem(String name, double p, double i, double d, double f, double period) {
super(name);
m_controller = new PIDController(p, i, d, f, m_source, m_output, period);
addChild("PIDController", m_controller);
}
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will use the
* class name as its name.
*
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
*/
public PIDSubsystem(double p, double i, double d) {
m_controller = new PIDController(p, i, d, m_source, m_output);
addChild("PIDController", m_controller);
}
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i, d, and f values. It will use
* the class name as its name. It will also space the time between PID loop calculations to be
* equal to the given period.
*
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param f the feed forward coefficient
* @param period the time (in seconds) between calculations
*/
public PIDSubsystem(double p, double i, double d, double f, double period) {
m_controller = new PIDController(p, i, d, f, m_source, m_output, period);
addChild("PIDController", m_controller);
}
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will use the
* class name as its name. It will also space the time between PID loop calculations to be equal
* to the given period.
*
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param period the time (in seconds) between calculations
*/
public PIDSubsystem(double p, double i, double d, double period) {
m_controller = new PIDController(p, i, d, m_source, m_output, period);
addChild("PIDController", m_controller);
}
/**
* Returns the {@link PIDController} used by this {@link PIDSubsystem}. Use this if you would like
* to fine tune the pid loop.
*
* @return the {@link PIDController} used by this {@link PIDSubsystem}
*/
public PIDController getPIDController() {
return m_controller;
}
/**
* Adds the given value to the setpoint. If {@link PIDSubsystem#setInputRange(double, double)
* setInputRange(...)} was used, then the bounds will still be honored by this method.
*
* @param deltaSetpoint the change in the setpoint
*/
public void setSetpointRelative(double deltaSetpoint) {
setSetpoint(getPosition() + deltaSetpoint);
}
/**
* Sets the setpoint to the given value. If {@link PIDSubsystem#setInputRange(double, double)
* setInputRange(...)} was called, then the given setpoint will be trimmed to fit within the
* range.
*
* @param setpoint the new setpoint
*/
public void setSetpoint(double setpoint) {
m_controller.setSetpoint(setpoint);
}
/**
* Returns the setpoint.
*
* @return the setpoint
*/
public double getSetpoint() {
return m_controller.getSetpoint();
}
/**
* Returns the current position.
*
* @return the current position
*/
public double getPosition() {
return returnPIDInput();
}
/**
* Sets the maximum and minimum values expected from the input.
*
* @param minimumInput the minimum value expected from the input
* @param maximumInput the maximum value expected from the output
*/
public void setInputRange(double minimumInput, double maximumInput) {
m_controller.setInputRange(minimumInput, maximumInput);
}
/**
* Sets the maximum and minimum values to write.
*
* @param minimumOutput the minimum value to write to the output
* @param maximumOutput the maximum value to write to the output
*/
public void setOutputRange(double minimumOutput, double maximumOutput) {
m_controller.setOutputRange(minimumOutput, maximumOutput);
}
/**
* Set the absolute error which is considered tolerable for use with OnTarget. The value is in the
* same range as the PIDInput values.
*
* @param t the absolute tolerance
*/
public void setAbsoluteTolerance(double t) {
m_controller.setAbsoluteTolerance(t);
}
/**
* Set the percentage error which is considered tolerable for use with OnTarget. (Value of 15.0 ==
* 15 percent).
*
* @param p the percent tolerance
*/
public void setPercentTolerance(double p) {
m_controller.setPercentTolerance(p);
}
/**
* Return true if the error is within the percentage of the total input range, determined by
* setTolerance. This assumes that the maximum and minimum input were set using setInput.
*
* @return true if the error is less than the tolerance
*/
public boolean onTarget() {
return m_controller.onTarget();
}
/**
* Returns the input for the pid loop.
*
* <p>It returns the input for the pid loop, so if this Subsystem was based off of a gyro, then it
* should return the angle of the gyro.
*
* <p>All subclasses of {@link PIDSubsystem} must override this method.
*
* @return the value the pid loop should use as input
*/
protected abstract double returnPIDInput();
/**
* Uses the value that the pid loop calculated. The calculated value is the "output" parameter.
* This method is a good time to set motor values, maybe something along the lines of <code>
* driveline.tankDrive(output, -output)</code>.
*
* <p>All subclasses of {@link PIDSubsystem} must override this method.
*
* @param output the value the pid loop calculated
*/
protected abstract void usePIDOutput(double output);
/** Enables the internal {@link PIDController}. */
public void enable() {
m_controller.enable();
}
/** Disables the internal {@link PIDController}. */
public void disable() {
m_controller.disable();
}
}

32
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java
View File

@@ -1,32 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
/**
* A {@link PrintCommand} is a command which prints out a string when it is initialized, and then
* immediately finishes. It is useful if you want a {@link CommandGroup} to print out a string when
* it reaches a certain point.
*
* <p>This class is provided by the OldCommands VendorDep
*/
public class PrintCommand extends InstantCommand {
/** The message to print out. */
private final String m_message;
/**
* Instantiates a {@link PrintCommand} which will print the given message when it is run.
*
* @param message the message to print
*/
public PrintCommand(String message) {
super("Print(\"" + message + "\"");
m_message = message;
}
@Override
protected void initialize() {
System.out.println(m_message);
}
}

347
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java
View File

@@ -1,347 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
import edu.wpi.first.hal.FRCNetComm.tInstances;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.networktables.NTSendable;
import edu.wpi.first.networktables.NTSendableBuilder;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import java.util.Enumeration;
import java.util.Hashtable;
import java.util.Map;
import java.util.Vector;
/**
* The {@link Scheduler} is a singleton which holds the top-level running commands. It is in charge
* of both calling the command's {@link Command#run() run()} method and to make sure that there are
* no two commands with conflicting requirements running.
*
* <p>It is fine if teams wish to take control of the {@link Scheduler} themselves, all that needs
* to be done is to call {@link Scheduler#getInstance() Scheduler.getInstance()}.{@link
* Scheduler#run() run()} often to have {@link Command Commands} function correctly. However, this
* is already done for you if you use the CommandBased Robot template.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @see Command
*/
public final class Scheduler implements NTSendable, AutoCloseable {
/** The Singleton Instance. */
private static Scheduler instance;
/**
* Returns the {@link Scheduler}, creating it if one does not exist.
*
* @return the {@link Scheduler}
*/
public static synchronized Scheduler getInstance() {
if (instance == null) {
instance = new Scheduler();
}
return instance;
}
/** A hashtable of active {@link Command Commands} to their {@link LinkedListElement}. */
private final Map<Command, LinkedListElement> m_commandTable = new Hashtable<>();
/** The {@link Set} of all {@link Subsystem Subsystems}. */
private final Set m_subsystems = new Set();
/** The first {@link Command} in the list. */
private LinkedListElement m_firstCommand;
/** The last {@link Command} in the list. */
private LinkedListElement m_lastCommand;
/** Whether or not we are currently adding a command. */
private boolean m_adding;
/** Whether or not we are currently disabled. */
private boolean m_disabled;
/** A list of all {@link Command Commands} which need to be added. */
@SuppressWarnings("PMD.UseArrayListInsteadOfVector")
private final Vector<Command> m_additions = new Vector<>();
/**
* A list of all {@link edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler Buttons}. It is
* created lazily.
*/
private Vector<ButtonScheduler> m_buttons;
private boolean m_runningCommandsChanged;
/** Instantiates a {@link Scheduler}. */
private Scheduler() {
HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler);
SendableRegistry.addLW(this, "Scheduler");
LiveWindow.setEnabledListener(
() -> {
disable();
removeAll();
});
LiveWindow.setDisabledListener(
() -> {
enable();
});
}
@Override
public void close() {
SendableRegistry.remove(this);
LiveWindow.setEnabledListener(null);
LiveWindow.setDisabledListener(null);
}
/**
* Adds the command to the {@link Scheduler}. This will not add the {@link Command} immediately,
* but will instead wait for the proper time in the {@link Scheduler#run()} loop before doing so.
* The command returns immediately and does nothing if given null.
*
* <p>Adding a {@link Command} to the {@link Scheduler} involves the {@link Scheduler} removing
* any {@link Command} which has shared requirements.
*
* @param command the command to add
*/
public void add(Command command) {
if (command != null) {
m_additions.addElement(command);
}
}
/**
* Adds a button to the {@link Scheduler}. The {@link Scheduler} will poll the button during its
* {@link Scheduler#run()}.
*
* @param button the button to add
*/
@SuppressWarnings("PMD.UseArrayListInsteadOfVector")
public void addButton(ButtonScheduler button) {
if (m_buttons == null) {
m_buttons = new Vector<>();
}
m_buttons.addElement(button);
}
/**
* Adds a command immediately to the {@link Scheduler}. This should only be called in the {@link
* Scheduler#run()} loop. Any command with conflicting requirements will be removed, unless it is
* uninterruptible. Giving <code>null</code> does nothing.
*
* @param command the {@link Command} to add
*/
@SuppressWarnings("MethodName")
private void _add(Command command) {
if (command == null) {
return;
}
// Check to make sure no adding during adding
if (m_adding) {
System.err.println("WARNING: Can not start command from cancel method. Ignoring:" + command);
return;
}
// Only add if not already in
if (!m_commandTable.containsKey(command)) {
// Check that the requirements can be had
Enumeration<?> requirements = command.getRequirements();
while (requirements.hasMoreElements()) {
Subsystem lock = (Subsystem) requirements.nextElement();
if (lock.getCurrentCommand() != null && !lock.getCurrentCommand().isInterruptible()) {
return;
}
}
// Give it the requirements
m_adding = true;
requirements = command.getRequirements();
while (requirements.hasMoreElements()) {
Subsystem lock = (Subsystem) requirements.nextElement();
if (lock.getCurrentCommand() != null) {
lock.getCurrentCommand().cancel();
remove(lock.getCurrentCommand());
}
lock.setCurrentCommand(command);
}
m_adding = false;
// Add it to the list
LinkedListElement element = new LinkedListElement();
element.setData(command);
if (m_firstCommand == null) {
m_firstCommand = m_lastCommand = element;
} else {
m_lastCommand.add(element);
m_lastCommand = element;
}
m_commandTable.put(command, element);
m_runningCommandsChanged = true;
command.startRunning();
}
}
/**
* Runs a single iteration of the loop. This method should be called often in order to have a
* functioning {@link Command} system. The loop has five stages:
*
* <ol>
* <li>Poll the Buttons
* <li>Execute/Remove the Commands
* <li>Send values to SmartDashboard
* <li>Add Commands
* <li>Add Defaults
* </ol>
*/
public void run() {
m_runningCommandsChanged = false;
if (m_disabled) {
return;
} // Don't run when m_disabled
// Get button input (going backwards preserves button priority)
if (m_buttons != null) {
for (int i = m_buttons.size() - 1; i >= 0; i--) {
m_buttons.elementAt(i).execute();
}
}
// Call every subsystem's periodic method
Enumeration<?> subsystems = m_subsystems.getElements();
while (subsystems.hasMoreElements()) {
((Subsystem) subsystems.nextElement()).periodic();
}
// Loop through the commands
LinkedListElement element = m_firstCommand;
while (element != null) {
Command command = element.getData();
element = element.getNext();
if (!command.run()) {
remove(command);
m_runningCommandsChanged = true;
}
}
// Add the new things
for (int i = 0; i < m_additions.size(); i++) {
_add(m_additions.elementAt(i));
}
m_additions.removeAllElements();
// Add in the defaults
Enumeration<?> locks = m_subsystems.getElements();
while (locks.hasMoreElements()) {
Subsystem lock = (Subsystem) locks.nextElement();
if (lock.getCurrentCommand() == null) {
_add(lock.getDefaultCommand());
}
lock.confirmCommand();
}
}
/**
* Registers a {@link Subsystem} to this {@link Scheduler}, so that the {@link Scheduler} might
* know if a default {@link Command} needs to be run. All {@link Subsystem Subsystems} should call
* this.
*
* @param system the system
*/
void registerSubsystem(Subsystem system) {
if (system != null) {
m_subsystems.add(system);
}
}
/**
* Removes the {@link Command} from the {@link Scheduler}.
*
* @param command the command to remove
*/
void remove(Command command) {
if (command == null || !m_commandTable.containsKey(command)) {
return;
}
LinkedListElement element = m_commandTable.get(command);
m_commandTable.remove(command);
if (element.equals(m_lastCommand)) {
m_lastCommand = element.getPrevious();
}
if (element.equals(m_firstCommand)) {
m_firstCommand = element.getNext();
}
element.remove();
Enumeration<?> requirements = command.getRequirements();
while (requirements.hasMoreElements()) {
((Subsystem) requirements.nextElement()).setCurrentCommand(null);
}
command.removed();
}
/** Removes all commands. */
public void removeAll() {
// TODO: Confirm that this works with "uninteruptible" commands
while (m_firstCommand != null) {
remove(m_firstCommand.getData());
}
}
/** Disable the command scheduler. */
public void disable() {
m_disabled = true;
}
/** Enable the command scheduler. */
public void enable() {
m_disabled = false;
}
@Override
public void initSendable(NTSendableBuilder builder) {
builder.setSmartDashboardType("Scheduler");
final NetworkTableEntry namesEntry = builder.getEntry("Names");
final NetworkTableEntry idsEntry = builder.getEntry("Ids");
final NetworkTableEntry cancelEntry = builder.getEntry("Cancel");
builder.setUpdateTable(
() -> {
if (namesEntry != null && idsEntry != null && cancelEntry != null) {
// Get the commands to cancel
double[] toCancel = cancelEntry.getDoubleArray(new double[0]);
if (toCancel.length > 0) {
for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
for (double d : toCancel) {
if (e.getData().hashCode() == d) {
e.getData().cancel();
}
}
}
cancelEntry.setDoubleArray(new double[0]);
}
if (m_runningCommandsChanged) {
// Set the the running commands
int number = 0;
for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
number++;
}
String[] commands = new String[number];
double[] ids = new double[number];
number = 0;
for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
commands[number] = e.getData().getName();
ids[number] = e.getData().hashCode();
number++;
}
namesEntry.setStringArray(commands);
idsEntry.setDoubleArray(ids);
}
}
});
}
}

46
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Set.java
View File

@@ -1,46 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
import java.util.Enumeration;
import java.util.Vector;
@SuppressWarnings("all")
/**
* A set.
*
* <p>This class is provided by the OldCommands VendorDep
*/
class Set {
private Vector m_set = new Vector();
public Set() {}
public void add(Object o) {
if (m_set.contains(o)) {
return;
}
m_set.addElement(o);
}
public void add(Set s) {
Enumeration stuff = s.getElements();
for (Enumeration e = stuff; e.hasMoreElements(); ) {
add(e.nextElement());
}
}
public void clear() {
m_set.clear();
}
public boolean contains(Object o) {
return m_set.contains(o);
}
public Enumeration getElements() {
return m_set.elements();
}
}

32
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java
View File

@@ -1,32 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
/**
* A {@link StartCommand} will call the {@link Command#start() start()} method of another command
* when it is initialized and will finish immediately.
*
* <p>This class is provided by the OldCommands VendorDep
*/
public class StartCommand extends InstantCommand {
/** The command to fork. */
private final Command m_commandToFork;
/**
* Instantiates a {@link StartCommand} which will start the given command whenever its {@link
* Command#initialize() initialize()} is called.
*
* @param commandToStart the {@link Command} to start
*/
public StartCommand(Command commandToStart) {
super("Start(" + commandToStart + ")");
m_commandToFork = commandToStart;
}
@Override
protected void initialize() {
m_commandToFork.start();
}
}

237
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java
View File

@@ -1,237 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
import java.util.Collections;
/**
* This class defines a major component of the robot.
*
* <p>A good example of a subsystem is the drivetrain, or a claw if the robot has one.
*
* <p>All motors should be a part of a subsystem. For instance, all the wheel motors should be a
* part of some kind of "Drivetrain" subsystem.
*
* <p>Subsystems are used within the command system as requirements for {@link Command}. Only one
* command which requires a subsystem can run at a time. Also, subsystems can have default commands
* which are started if there is no command running which requires this subsystem.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @see Command
*/
public abstract class Subsystem implements Sendable, AutoCloseable {
/** Whether or not getDefaultCommand() was called. */
private boolean m_initializedDefaultCommand;
/** The current command. */
private Command m_currentCommand;
private boolean m_currentCommandChanged;
/** The default command. */
private Command m_defaultCommand;
/**
* Creates a subsystem with the given name.
*
* @param name the name of the subsystem
*/
public Subsystem(String name) {
SendableRegistry.addLW(this, name, name);
Scheduler.getInstance().registerSubsystem(this);
}
/** Creates a subsystem. This will set the name to the name of the class. */
public Subsystem() {
String name = getClass().getName();
name = name.substring(name.lastIndexOf('.') + 1);
SendableRegistry.addLW(this, name, name);
Scheduler.getInstance().registerSubsystem(this);
m_currentCommandChanged = true;
}
@Override
public void close() {
SendableRegistry.remove(this);
}
/**
* Initialize the default command for a subsystem By default subsystems have no default command,
* but if they do, the default command is set with this method. It is called on all Subsystems by
* CommandBase in the users program after all the Subsystems are created.
*/
protected abstract void initDefaultCommand();
/** When the run method of the scheduler is called this method will be called. */
public void periodic() {
// Override me!
}
/**
* Sets the default command. If this is not called or is called with null, then there will be no
* default command for the subsystem.
*
* <p><b>WARNING:</b> This should <b>NOT</b> be called in a constructor if the subsystem is a
* singleton.
*
* @param command the default command (or null if there should be none)
* @throws IllegalUseOfCommandException if the command does not require the subsystem
*/
public void setDefaultCommand(Command command) {
if (command == null) {
m_defaultCommand = null;
} else {
if (!Collections.list(command.getRequirements()).contains(this)) {
throw new IllegalUseOfCommandException("A default command must require the subsystem");
}
m_defaultCommand = command;
}
}
/**
* Returns the default command (or null if there is none).
*
* @return the default command
*/
public Command getDefaultCommand() {
if (!m_initializedDefaultCommand) {
m_initializedDefaultCommand = true;
initDefaultCommand();
}
return m_defaultCommand;
}
/**
* Returns the default command name, or empty string is there is none.
*
* @return the default command name
*/
public String getDefaultCommandName() {
Command defaultCommand = getDefaultCommand();
if (defaultCommand != null) {
return defaultCommand.getName();
} else {
return "";
}
}
/**
* Sets the current command.
*
* @param command the new current command
*/
void setCurrentCommand(Command command) {
m_currentCommand = command;
m_currentCommandChanged = true;
}
/**
* Call this to alert Subsystem that the current command is actually the command. Sometimes, the
* {@link Subsystem} is told that it has no command while the {@link Scheduler} is going through
* the loop, only to be soon after given a new one. This will avoid that situation.
*/
void confirmCommand() {
if (m_currentCommandChanged) {
m_currentCommandChanged = false;
}
}
/**
* Returns the command which currently claims this subsystem.
*
* @return the command which currently claims this subsystem
*/
public Command getCurrentCommand() {
return m_currentCommand;
}
/**
* Returns the current command name, or empty string if no current command.
*
* @return the current command name
*/
public String getCurrentCommandName() {
Command currentCommand = getCurrentCommand();
if (currentCommand != null) {
return currentCommand.getName();
} else {
return "";
}
}
/**
* Associate a {@link Sendable} with this Subsystem. Also update the child's name.
*
* @param name name to give child
* @param child sendable
*/
public void addChild(String name, Sendable child) {
SendableRegistry.addLW(child, getSubsystem(), name);
}
/**
* Associate a {@link Sendable} with this Subsystem.
*
* @param child sendable
*/
public void addChild(Sendable child) {
SendableRegistry.setSubsystem(child, getSubsystem());
SendableRegistry.enableLiveWindow(child);
}
/**
* Gets the name of this Subsystem.
*
* @return Name
*/
public String getName() {
return SendableRegistry.getName(this);
}
/**
* Sets the name of this Subsystem.
*
* @param name name
*/
public void setName(String name) {
SendableRegistry.setName(this, name);
}
/**
* Gets the subsystem name of this Subsystem.
*
* @return Subsystem name
*/
public String getSubsystem() {
return SendableRegistry.getSubsystem(this);
}
/**
* Sets the subsystem name of this Subsystem.
*
* @param subsystem subsystem name
*/
public void setSubsystem(String subsystem) {
SendableRegistry.setSubsystem(this, subsystem);
}
@Override
public String toString() {
return getSubsystem();
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Subsystem");
builder.addBooleanProperty(".hasDefault", () -> m_defaultCommand != null, null);
builder.addStringProperty(".default", this::getDefaultCommandName, null);
builder.addBooleanProperty(".hasCommand", () -> m_currentCommand != null, null);
builder.addStringProperty(".command", this::getCurrentCommandName, null);
}
}

59
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java
View File

@@ -1,59 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
/**
* A {@link TimedCommand} will wait for a timeout before finishing. {@link TimedCommand} is used to
* execute a command for a given amount of time.
*
* <p>This class is provided by the OldCommands VendorDep
*/
public class TimedCommand extends Command {
/**
* Instantiates a TimedCommand with the given name and timeout.
*
* @param name the name of the command
* @param timeout the time the command takes to run (seconds)
*/
public TimedCommand(String name, double timeout) {
super(name, timeout);
}
/**
* Instantiates a TimedCommand with the given timeout.
*
* @param timeout the time the command takes to run (seconds)
*/
public TimedCommand(double timeout) {
super(timeout);
}
/**
* Instantiates a TimedCommand with the given name and timeout.
*
* @param name the name of the command
* @param timeout the time the command takes to run (seconds)
* @param subsystem the subsystem that this command requires
*/
public TimedCommand(String name, double timeout, Subsystem subsystem) {
super(name, timeout, subsystem);
}
/**
* Instantiates a TimedCommand with the given timeout.
*
* @param timeout the time the command takes to run (seconds)
* @param subsystem the subsystem that this command requires
*/
public TimedCommand(double timeout, Subsystem subsystem) {
super(timeout, subsystem);
}
/** Ends command when timed out. */
@Override
protected boolean isFinished() {
return isTimedOut();
}
}

34
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java
View File

@@ -1,34 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
/**
* A {@link WaitCommand} will wait for a certain amount of time before finishing. It is useful if
* you want a {@link CommandGroup} to pause for a moment.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @see CommandGroup
*/
public class WaitCommand extends TimedCommand {
/**
* Instantiates a {@link WaitCommand} with the given timeout.
*
* @param timeout the time the command takes to run (seconds)
*/
public WaitCommand(double timeout) {
this("Wait(" + timeout + ")", timeout);
}
/**
* Instantiates a {@link WaitCommand} with the given timeout.
*
* @param name the name of the command
* @param timeout the time the command takes to run (seconds)
*/
public WaitCommand(String name, double timeout) {
super(name, timeout);
}
}

22
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java
View File

@@ -1,22 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
/**
* This command will only finish if whatever {@link CommandGroup} it is in has no active children.
* If it is not a part of a {@link CommandGroup}, then it will finish immediately. If it is itself
* an active child, then the {@link CommandGroup} will never end.
*
* <p>This class is useful for the situation where you want to allow anything running in parallel to
* finish, before continuing in the main {@link CommandGroup} sequence.
*
* <p>This class is provided by the OldCommands VendorDep
*/
public class WaitForChildren extends Command {
@Override
protected boolean isFinished() {
return getGroup() == null || getGroup().m_children.isEmpty();
}
}

28
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java
View File

@@ -1,28 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.command;
import edu.wpi.first.wpilibj.Timer;
/**
* WaitUntilCommand - waits until an absolute game time. This will wait until the game clock reaches
* some value, then continue to the next command.
*
* <p>This class is provided by the OldCommands VendorDep
*/
public class WaitUntilCommand extends Command {
private final double m_time;
public WaitUntilCommand(double time) {
super("WaitUntil(" + time + ")");
m_time = time;
}
/** Check if we've reached the actual finish time. */
@Override
public boolean isFinished() {
return Timer.getMatchTime() >= m_time;
}
}

51
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogAccelerometer.java
View File

@@ -1,51 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.pidwrappers;
import edu.wpi.first.wpilibj.AnalogAccelerometer;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
/**
* Wrapper so that PIDSource is implemented for AnalogAccelerometer for old PIDController.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @deprecated Use {@link edu.wpi.first.math.controller.PIDController} which doesn't require this
* wrapper.
*/
@Deprecated(since = "2022", forRemoval = true)
public class PIDAnalogAccelerometer extends AnalogAccelerometer implements PIDSource {
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
public PIDAnalogAccelerometer(int channel) {
super(channel);
}
public PIDAnalogAccelerometer(AnalogInput channel) {
super(channel);
}
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the Acceleration for the PID Source parent.
*
* @return The current acceleration in Gs.
*/
@Override
public double pidGet() {
return getAcceleration();
}
}

73
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogGyro.java
View File

@@ -1,73 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.pidwrappers;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
/**
* Wrapper so that PIDSource is implemented for AnalogGyro for old PIDController.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @deprecated Use {@link edu.wpi.first.math.controller.PIDController} which doesn't require this
* wrapper.
*/
@Deprecated(since = "2022", forRemoval = true)
public class PIDAnalogGyro extends AnalogGyro implements PIDSource {
private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
public PIDAnalogGyro(int channel) {
super(channel);
}
public PIDAnalogGyro(AnalogInput channel) {
super(channel);
}
public PIDAnalogGyro(int channel, int center, double offset) {
super(channel, center, offset);
}
public PIDAnalogGyro(AnalogInput channel, int center, double offset) {
super(channel, center, offset);
}
/**
* Set which parameter of the gyro you are using as a process control variable. The Gyro class
* supports the rate and displacement parameters
*
* @param pidSource An enum to select the parameter.
*/
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the output of the gyro for use with PIDControllers. May be the angle or rate depending on
* the set PIDSourceType
*
* @return the output according to the gyro
*/
@Override
public double pidGet() {
switch (m_pidSource) {
case kRate:
return getRate();
case kDisplacement:
return getAngle();
default:
return 0.0;
}
}
}

46
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogInput.java
View File

@@ -1,46 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.pidwrappers;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
/**
* Wrapper so that PIDSource is implemented for AnalogInput for old PIDController.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @deprecated Use {@link edu.wpi.first.math.controller.PIDController} which doesn't require this
* wrapper.
*/
@Deprecated(since = "2022", forRemoval = true)
public class PIDAnalogInput extends AnalogInput implements PIDSource {
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
public PIDAnalogInput(int channel) {
super(channel);
}
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the average voltage for use with PIDController.
*
* @return the average voltage
*/
@Override
public double pidGet() {
return getAverageVoltage();
}
}

70
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogPotentiometer.java
View File

@@ -1,70 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.pidwrappers;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
/**
* Wrapper so that PIDSource is implemented for AnalogPotentiometer for old PIDController.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @deprecated Use {@link edu.wpi.first.math.controller.PIDController} which doesn't require this
* wrapper.
*/
@Deprecated(since = "2022", forRemoval = true)
public class PIDAnalogPotentiometer extends AnalogPotentiometer implements PIDSource {
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
public PIDAnalogPotentiometer(int channel, double fullRange, double offset) {
super(channel, fullRange, offset);
}
public PIDAnalogPotentiometer(AnalogInput input, double fullRange, double offset) {
super(input, fullRange, offset);
}
public PIDAnalogPotentiometer(int channel, double scale) {
super(channel, scale);
}
public PIDAnalogPotentiometer(AnalogInput input, double scale) {
super(input, scale);
}
public PIDAnalogPotentiometer(int channel) {
super(channel);
}
public PIDAnalogPotentiometer(AnalogInput input) {
super(input);
}
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
if (!pidSource.equals(PIDSourceType.kDisplacement)) {
throw new IllegalArgumentException("Only displacement PID is allowed for potentiometers.");
}
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Implement the PIDSource interface.
*
* @return The current reading.
*/
@Override
public double pidGet() {
return get();
}
}

109
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDEncoder.java
View File

@@ -1,109 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.pidwrappers;
import edu.wpi.first.wpilibj.DigitalSource;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
/**
* Wrapper so that PIDSource is implemented for Encoder for old PIDController.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @deprecated Use {@link edu.wpi.first.math.controller.PIDController} which doesn't require this
* wrapper.
*/
@Deprecated(since = "2022", forRemoval = true)
public class PIDEncoder extends Encoder implements PIDSource {
private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
public PIDEncoder(final int channelA, final int channelB, boolean reverseDirection) {
super(channelA, channelB, reverseDirection, EncodingType.k4X);
}
public PIDEncoder(final int channelA, final int channelB) {
super(channelA, channelB, false);
}
public PIDEncoder(
final int channelA,
final int channelB,
boolean reverseDirection,
final EncodingType encodingType) {
super(channelA, channelB, reverseDirection, encodingType);
}
public PIDEncoder(
final int channelA, final int channelB, final int indexChannel, boolean reverseDirection) {
super(channelA, channelB, indexChannel, reverseDirection);
}
public PIDEncoder(final int channelA, final int channelB, final int indexChannel) {
super(channelA, channelB, indexChannel, false);
}
public PIDEncoder(DigitalSource sourceA, DigitalSource sourceB, boolean reverseDirection) {
super(sourceA, sourceB, reverseDirection, EncodingType.k4X);
}
public PIDEncoder(DigitalSource sourceA, DigitalSource sourceB) {
super(sourceA, sourceB, false);
}
public PIDEncoder(
DigitalSource sourceA,
DigitalSource sourceB,
boolean reverseDirection,
final EncodingType encodingType) {
super(sourceA, sourceB, reverseDirection, encodingType);
}
public PIDEncoder(
DigitalSource sourceA,
DigitalSource sourceB,
DigitalSource indexSource,
boolean reverseDirection) {
super(sourceA, sourceB, indexSource, reverseDirection);
}
public PIDEncoder(DigitalSource sourceA, DigitalSource sourceB, DigitalSource indexSource) {
super(sourceA, sourceB, indexSource, false);
}
/**
* Set which parameter of the encoder you are using as a process control variable. The encoder
* class supports the rate and distance parameters.
*
* @param pidSource An enum to select the parameter.
*/
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Implement the PIDSource interface.
*
* @return The current value of the selected source parameter.
*/
@Override
public double pidGet() {
switch (m_pidSource) {
case kDisplacement:
return getDistance();
case kRate:
return getRate();
default:
return 0.0;
}
}
}

116
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDMotorController.java
View File

@@ -1,116 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.pidwrappers;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
/**
* Wrapper so that PIDOutput is implemented for MotorController for old PIDController.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @deprecated Use {@link edu.wpi.first.math.controller.PIDController} which doesn't require this
* wrapper.
*/
@Deprecated(since = "2022", forRemoval = true)
public class PIDMotorController implements PIDOutput, MotorController, Sendable {
private final MotorController m_motorController;
public PIDMotorController(MotorController motorController) {
m_motorController = motorController;
}
/**
* Write out the PID value as seen in the PIDOutput base object.
*
* @param output Write out the PWM value as was found in the PIDController
*/
@Override
public void pidWrite(double output) {
m_motorController.set(output);
}
/**
* Common interface for setting the speed of a motor controller.
*
* @param speed The speed to set. Value should be between -1.0 and 1.0.
*/
@Override
public void set(double speed) {
m_motorController.set(speed);
}
/**
* Sets the voltage output of the MotorController. Compensates for the current bus voltage to
* ensure that the desired voltage is output even if the battery voltage is below 12V - highly
* useful when the voltage outputs are "meaningful" (e.g. they come from a feedforward
* calculation).
*
* <p>NOTE: This function *must* be called regularly in order for voltage compensation to work
* properly - unlike the ordinary set function, it is not "set it and forget it."
*
* @param outputVolts The voltage to output.
*/
@Override
public void setVoltage(double outputVolts) {
m_motorController.setVoltage(outputVolts);
}
/**
* Common interface for getting the current set speed of a motor controller.
*
* @return The current set speed. Value is between -1.0 and 1.0.
*/
@Override
public double get() {
return m_motorController.get();
}
/**
* Common interface for inverting direction of a motor controller.
*
* @param isInverted The state of inversion true is inverted.
*/
@Override
public void setInverted(boolean isInverted) {
m_motorController.setInverted(isInverted);
}
/**
* Common interface for returning if a motor controller is in the inverted state or not.
*
* @return isInverted The state of the inversion true is inverted.
*/
@Override
public boolean getInverted() {
return m_motorController.getInverted();
}
/** Disable the motor controller. */
@Override
public void disable() {
m_motorController.disable();
}
/**
* Stops motor movement. Motor can be moved again by calling set without having to re-enable the
* motor.
*/
@Override
public void stopMotor() {
m_motorController.stopMotor();
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Motor Controller");
builder.setActuator(true);
builder.setSafeState(this::disable);
builder.addDoubleProperty("Value", this::get, this::set);
}
}

55
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDUltrasonic.java
View File

@@ -1,55 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.pidwrappers;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.Ultrasonic;
/**
* Wrapper so that PIDSource is implemented for Ultrasonic for old PIDController.
*
* <p>This class is provided by the OldCommands VendorDep
*
* @deprecated Use {@link edu.wpi.first.math.controller.PIDController} which doesn't require this
* wrapper.
*/
@Deprecated(since = "2022", forRemoval = true)
public class PIDUltrasonic extends Ultrasonic implements PIDSource {
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
public PIDUltrasonic(int pingChannel, int echoChannel) {
super(pingChannel, echoChannel);
}
public PIDUltrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) {
super(pingChannel, echoChannel);
}
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
if (!pidSource.equals(PIDSourceType.kDisplacement)) {
throw new IllegalArgumentException("Only displacement PID is allowed for ultrasonics.");
}
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the range in the inches for the PIDSource base object.
*
* @return The range in inches
*/
@Override
public double pidGet() {
return getRangeInches();
}
}

358
wpilibOldCommands/src/main/native/cpp/PIDBase.cpp
View File

@@ -1,358 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/PIDBase.h"
#include <algorithm>
#include <cmath>
#include <hal/FRCUsageReporting.h>
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
#include "frc/PIDOutput.h"
using namespace frc;
template <class T>
constexpr const T& clamp(const T& value, const T& low, const T& high) {
return std::max(low, std::min(value, high));
}
PIDBase::PIDBase(double Kp, double Ki, double Kd, PIDSource& source,
PIDOutput& output)
: PIDBase(Kp, Ki, Kd, 0.0, source, output) {}
PIDBase::PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource& source,
PIDOutput& output) {
m_P = Kp;
m_I = Ki;
m_D = Kd;
m_F = Kf;
m_pidInput = &source;
m_filter = LinearFilter<double>::MovingAverage(1);
m_pidOutput = &output;
m_setpointTimer.Start();
static int instances = 0;
instances++;
HAL_Report(HALUsageReporting::kResourceType_PIDController, instances);
wpi::SendableRegistry::Add(this, "PIDController", instances);
}
double PIDBase::Get() const {
std::scoped_lock lock(m_thisMutex);
return m_result;
}
void PIDBase::SetContinuous(bool continuous) {
std::scoped_lock lock(m_thisMutex);
m_continuous = continuous;
}
void PIDBase::SetInputRange(double minimumInput, double maximumInput) {
{
std::scoped_lock lock(m_thisMutex);
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
m_inputRange = maximumInput - minimumInput;
}
SetSetpoint(m_setpoint);
}
void PIDBase::SetOutputRange(double minimumOutput, double maximumOutput) {
std::scoped_lock lock(m_thisMutex);
m_minimumOutput = minimumOutput;
m_maximumOutput = maximumOutput;
}
void PIDBase::SetPID(double p, double i, double d) {
{
std::scoped_lock lock(m_thisMutex);
m_P = p;
m_I = i;
m_D = d;
}
}
void PIDBase::SetPID(double p, double i, double d, double f) {
std::scoped_lock lock(m_thisMutex);
m_P = p;
m_I = i;
m_D = d;
m_F = f;
}
void PIDBase::SetP(double p) {
std::scoped_lock lock(m_thisMutex);
m_P = p;
}
void PIDBase::SetI(double i) {
std::scoped_lock lock(m_thisMutex);
m_I = i;
}
void PIDBase::SetD(double d) {
std::scoped_lock lock(m_thisMutex);
m_D = d;
}
void PIDBase::SetF(double f) {
std::scoped_lock lock(m_thisMutex);
m_F = f;
}
double PIDBase::GetP() const {
std::scoped_lock lock(m_thisMutex);
return m_P;
}
double PIDBase::GetI() const {
std::scoped_lock lock(m_thisMutex);
return m_I;
}
double PIDBase::GetD() const {
std::scoped_lock lock(m_thisMutex);
return m_D;
}
double PIDBase::GetF() const {
std::scoped_lock lock(m_thisMutex);
return m_F;
}
void PIDBase::SetSetpoint(double setpoint) {
{
std::scoped_lock lock(m_thisMutex);
if (m_maximumInput > m_minimumInput) {
if (setpoint > m_maximumInput) {
m_setpoint = m_maximumInput;
} else if (setpoint < m_minimumInput) {
m_setpoint = m_minimumInput;
} else {
m_setpoint = setpoint;
}
} else {
m_setpoint = setpoint;
}
}
}
double PIDBase::GetSetpoint() const {
std::scoped_lock lock(m_thisMutex);
return m_setpoint;
}
double PIDBase::GetDeltaSetpoint() const {
std::scoped_lock lock(m_thisMutex);
return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get().value();
}
double PIDBase::GetError() const {
double setpoint = GetSetpoint();
{
std::scoped_lock lock(m_thisMutex);
return GetContinuousError(setpoint - m_pidInput->PIDGet());
}
}
double PIDBase::GetAvgError() const {
return GetError();
}
void PIDBase::SetPIDSourceType(PIDSourceType pidSource) {
m_pidInput->SetPIDSourceType(pidSource);
}
PIDSourceType PIDBase::GetPIDSourceType() const {
return m_pidInput->GetPIDSourceType();
}
void PIDBase::SetTolerance(double percent) {
std::scoped_lock lock(m_thisMutex);
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
void PIDBase::SetAbsoluteTolerance(double absTolerance) {
std::scoped_lock lock(m_thisMutex);
m_toleranceType = kAbsoluteTolerance;
m_tolerance = absTolerance;
}
void PIDBase::SetPercentTolerance(double percent) {
std::scoped_lock lock(m_thisMutex);
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
void PIDBase::SetToleranceBuffer(int bufLength) {
std::scoped_lock lock(m_thisMutex);
m_filter = LinearFilter<double>::MovingAverage(bufLength);
}
bool PIDBase::OnTarget() const {
double error = GetError();
std::scoped_lock lock(m_thisMutex);
switch (m_toleranceType) {
case kPercentTolerance:
return std::fabs(error) < m_tolerance / 100 * m_inputRange;
break;
case kAbsoluteTolerance:
return std::fabs(error) < m_tolerance;
break;
case kNoTolerance:
// TODO: this case needs an error
return false;
}
return false;
}
void PIDBase::Reset() {
std::scoped_lock lock(m_thisMutex);
m_prevError = 0;
m_totalError = 0;
m_result = 0;
}
void PIDBase::PIDWrite(double output) {
SetSetpoint(output);
}
void PIDBase::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("PIDController");
builder.SetSafeState([=] { Reset(); });
builder.AddDoubleProperty(
"p", [=] { return GetP(); }, [=](double value) { SetP(value); });
builder.AddDoubleProperty(
"i", [=] { return GetI(); }, [=](double value) { SetI(value); });
builder.AddDoubleProperty(
"d", [=] { return GetD(); }, [=](double value) { SetD(value); });
builder.AddDoubleProperty(
"f", [=] { return GetF(); }, [=](double value) { SetF(value); });
builder.AddDoubleProperty(
"setpoint", [=] { return GetSetpoint(); },
[=](double value) { SetSetpoint(value); });
}
void PIDBase::Calculate() {
if (m_pidInput == nullptr || m_pidOutput == nullptr) {
return;
}
bool enabled;
{
std::scoped_lock lock(m_thisMutex);
enabled = m_enabled;
}
if (enabled) {
double input;
// Storage for function inputs
PIDSourceType pidSourceType;
double P;
double I;
double D;
double feedForward = CalculateFeedForward();
double minimumOutput;
double maximumOutput;
// Storage for function input-outputs
double prevError;
double error;
double totalError;
{
std::scoped_lock lock(m_thisMutex);
input = m_filter.Calculate(m_pidInput->PIDGet());
pidSourceType = m_pidInput->GetPIDSourceType();
P = m_P;
I = m_I;
D = m_D;
minimumOutput = m_minimumOutput;
maximumOutput = m_maximumOutput;
prevError = m_prevError;
error = GetContinuousError(m_setpoint - input);
totalError = m_totalError;
}
// Storage for function outputs
double result;
if (pidSourceType == PIDSourceType::kRate) {
if (P != 0) {
totalError =
clamp(totalError + error, minimumOutput / P, maximumOutput / P);
}
result = D * error + P * totalError + feedForward;
} else {
if (I != 0) {
totalError =
clamp(totalError + error, minimumOutput / I, maximumOutput / I);
}
result =
P * error + I * totalError + D * (error - prevError) + feedForward;
}
result = clamp(result, minimumOutput, maximumOutput);
{
// Ensures m_enabled check and PIDWrite() call occur atomically
std::scoped_lock pidWriteLock(m_pidWriteMutex);
std::unique_lock mainLock(m_thisMutex);
if (m_enabled) {
// Don't block other PIDBase operations on PIDWrite()
mainLock.unlock();
m_pidOutput->PIDWrite(result);
}
}
std::scoped_lock lock(m_thisMutex);
m_prevError = m_error;
m_error = error;
m_totalError = totalError;
m_result = result;
}
}
double PIDBase::CalculateFeedForward() {
if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
return m_F * GetSetpoint();
} else {
double temp = m_F * GetDeltaSetpoint();
m_prevSetpoint = m_setpoint;
m_setpointTimer.Reset();
return temp;
}
}
double PIDBase::GetContinuousError(double error) const {
if (m_continuous && m_inputRange != 0) {
error = std::fmod(error, m_inputRange);
if (std::fabs(error) > m_inputRange / 2) {
if (error > 0) {
return error - m_inputRange;
} else {
return error + m_inputRange;
}
}
}
return error;
}

84
wpilibOldCommands/src/main/native/cpp/PIDController.cpp
View File

@@ -1,84 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/PIDController.h"
#include <wpi/sendable/SendableBuilder.h>
#include "frc/Notifier.h"
#include "frc/PIDOutput.h"
using namespace frc;
PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource* source,
PIDOutput* output, double period)
: PIDController(Kp, Ki, Kd, 0.0, *source, *output, period) {}
PIDController::PIDController(double Kp, double Ki, double Kd, double Kf,
PIDSource* source, PIDOutput* output,
double period)
: PIDController(Kp, Ki, Kd, Kf, *source, *output, period) {}
PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource& source,
PIDOutput& output, double period)
: PIDController(Kp, Ki, Kd, 0.0, source, output, period) {}
PIDController::PIDController(double Kp, double Ki, double Kd, double Kf,
PIDSource& source, PIDOutput& output,
double period)
: PIDBase(Kp, Ki, Kd, Kf, source, output) {
m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
m_controlLoop->StartPeriodic(units::second_t(period));
}
PIDController::~PIDController() {
// Forcefully stopping the notifier so the callback can successfully run.
m_controlLoop->Stop();
}
void PIDController::Enable() {
{
std::scoped_lock lock(m_thisMutex);
m_enabled = true;
}
}
void PIDController::Disable() {
{
// Ensures m_enabled modification and PIDWrite() call occur atomically
std::scoped_lock pidWriteLock(m_pidWriteMutex);
{
std::scoped_lock mainLock(m_thisMutex);
m_enabled = false;
}
m_pidOutput->PIDWrite(0);
}
}
void PIDController::SetEnabled(bool enable) {
if (enable) {
Enable();
} else {
Disable();
}
}
bool PIDController::IsEnabled() const {
std::scoped_lock lock(m_thisMutex);
return m_enabled;
}
void PIDController::Reset() {
Disable();
PIDBase::Reset();
}
void PIDController::InitSendable(wpi::SendableBuilder& builder) {
PIDBase::InitSendable(builder);
builder.AddBooleanProperty(
"enabled", [=] { return IsEnabled(); },
[=](bool value) { SetEnabled(value); });
}

15
wpilibOldCommands/src/main/native/cpp/PIDSource.cpp
View File

@@ -1,15 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/PIDSource.h"
using namespace frc;
void PIDSource::SetPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
PIDSourceType PIDSource::GetPIDSourceType() const {
return m_pidSource;
}

27
wpilibOldCommands/src/main/native/cpp/buttons/Button.cpp
View File

@@ -1,27 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/buttons/Button.h"
using namespace frc;
void Button::WhenPressed(Command* command) {
WhenActive(command);
}
void Button::WhileHeld(Command* command) {
WhileActive(command);
}
void Button::WhenReleased(Command* command) {
WhenInactive(command);
}
void Button::CancelWhenPressed(Command* command) {
CancelWhenActive(command);
}
void Button::ToggleWhenPressed(Command* command) {
ToggleWhenActive(command);
}

16
wpilibOldCommands/src/main/native/cpp/buttons/ButtonScheduler.cpp
View File

@@ -1,16 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/buttons/ButtonScheduler.h"
#include "frc/commands/Scheduler.h"
using namespace frc;
ButtonScheduler::ButtonScheduler(bool last, Trigger* button, Command* orders)
: m_pressedLast(last), m_button(button), m_command(orders) {}
void ButtonScheduler::Start() {
Scheduler::GetInstance()->AddButton(this);
}

24
wpilibOldCommands/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
View File

@@ -1,24 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/buttons/CancelButtonScheduler.h"
#include "frc/buttons/Button.h"
#include "frc/commands/Command.h"
using namespace frc;
CancelButtonScheduler::CancelButtonScheduler(bool last, Trigger* button,
Command* orders)
: ButtonScheduler(last, button, orders) {}
void CancelButtonScheduler::Execute() {
bool pressed = m_button->Grab();
if (!m_pressedLast && pressed) {
m_command->Cancel();
}
m_pressedLast = pressed;
}

26
wpilibOldCommands/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
View File

@@ -1,26 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/buttons/HeldButtonScheduler.h"
#include "frc/buttons/Button.h"
#include "frc/commands/Command.h"
using namespace frc;
HeldButtonScheduler::HeldButtonScheduler(bool last, Trigger* button,
Command* orders)
: ButtonScheduler(last, button, orders) {}
void HeldButtonScheduler::Execute() {
bool pressed = m_button->Grab();
if (pressed) {
m_command->Start();
} else if (m_pressedLast && !pressed) {
m_command->Cancel();
}
m_pressedLast = pressed;
}

22
wpilibOldCommands/src/main/native/cpp/buttons/InternalButton.cpp
View File

@@ -1,22 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/buttons/InternalButton.h"
using namespace frc;
InternalButton::InternalButton(bool inverted)
: m_pressed(inverted), m_inverted(inverted) {}
void InternalButton::SetInverted(bool inverted) {
m_inverted = inverted;
}
void InternalButton::SetPressed(bool pressed) {
m_pressed = pressed;
}
bool InternalButton::Get() {
return m_pressed ^ m_inverted;
}

14
wpilibOldCommands/src/main/native/cpp/buttons/JoystickButton.cpp
View File

@@ -1,14 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/buttons/JoystickButton.h"
using namespace frc;
JoystickButton::JoystickButton(GenericHID* joystick, int buttonNumber)
: m_joystick(joystick), m_buttonNumber(buttonNumber) {}
bool JoystickButton::Get() {
return m_joystick->GetRawButton(m_buttonNumber);
}

22
wpilibOldCommands/src/main/native/cpp/buttons/NetworkButton.cpp
View File

@@ -1,22 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/buttons/NetworkButton.h"
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableInstance.h>
using namespace frc;
NetworkButton::NetworkButton(std::string_view tableName, std::string_view field)
: NetworkButton(nt::NetworkTableInstance::GetDefault().GetTable(tableName),
field) {}
NetworkButton::NetworkButton(std::shared_ptr<nt::NetworkTable> table,
std::string_view field)
: m_entry(table->GetEntry(field)) {}
bool NetworkButton::Get() {
return m_entry.GetInstance().IsConnected() && m_entry.GetBoolean(false);
}

14
wpilibOldCommands/src/main/native/cpp/buttons/POVButton.cpp
View File

@@ -1,14 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/buttons/POVButton.h"
using namespace frc;
POVButton::POVButton(GenericHID& joystick, int angle, int povNumber)
: m_joystick(&joystick), m_angle(angle), m_povNumber(povNumber) {}
bool POVButton::Get() {
return m_joystick->GetPOV(m_povNumber) == m_angle;
}

24
wpilibOldCommands/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
View File

@@ -1,24 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/buttons/PressedButtonScheduler.h"
#include "frc/buttons/Button.h"
#include "frc/commands/Command.h"
using namespace frc;
PressedButtonScheduler::PressedButtonScheduler(bool last, Trigger* button,
Command* orders)
: ButtonScheduler(last, button, orders) {}
void PressedButtonScheduler::Execute() {
bool pressed = m_button->Grab();
if (!m_pressedLast && pressed) {
m_command->Start();
}
m_pressedLast = pressed;
}

24
wpilibOldCommands/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
View File

@@ -1,24 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/buttons/ReleasedButtonScheduler.h"
#include "frc/buttons/Button.h"
#include "frc/commands/Command.h"
using namespace frc;
ReleasedButtonScheduler::ReleasedButtonScheduler(bool last, Trigger* button,
Command* orders)
: ButtonScheduler(last, button, orders) {}
void ReleasedButtonScheduler::Execute() {
bool pressed = m_button->Grab();
if (m_pressedLast && !pressed) {
m_command->Start();
}
m_pressedLast = pressed;
}

28
wpilibOldCommands/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
View File

@@ -1,28 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/buttons/ToggleButtonScheduler.h"
#include "frc/buttons/Button.h"
#include "frc/commands/Command.h"
using namespace frc;
ToggleButtonScheduler::ToggleButtonScheduler(bool last, Trigger* button,
Command* orders)
: ButtonScheduler(last, button, orders) {}
void ToggleButtonScheduler::Execute() {
bool pressed = m_button->Grab();
if (!m_pressedLast && pressed) {
if (m_command->IsRunning()) {
m_command->Cancel();
} else {
m_command->Start();
}
}
m_pressedLast = pressed;
}

72
wpilibOldCommands/src/main/native/cpp/buttons/Trigger.cpp
View File

@@ -1,72 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <wpi/sendable/SendableBuilder.h>
#include "frc/buttons/Button.h"
#include "frc/buttons/CancelButtonScheduler.h"
#include "frc/buttons/HeldButtonScheduler.h"
#include "frc/buttons/PressedButtonScheduler.h"
#include "frc/buttons/ReleasedButtonScheduler.h"
#include "frc/buttons/ToggleButtonScheduler.h"
using namespace frc;
Trigger::Trigger(const Trigger& rhs) : SendableHelper(rhs) {}
Trigger& Trigger::operator=(const Trigger& rhs) {
SendableHelper::operator=(rhs);
m_sendablePressed = false;
return *this;
}
Trigger::Trigger(Trigger&& rhs)
: SendableHelper(std::move(rhs)),
m_sendablePressed(rhs.m_sendablePressed.load()) {
rhs.m_sendablePressed = false;
}
Trigger& Trigger::operator=(Trigger&& rhs) {
SendableHelper::operator=(std::move(rhs));
m_sendablePressed = rhs.m_sendablePressed.load();
rhs.m_sendablePressed = false;
return *this;
}
bool Trigger::Grab() {
return Get() || m_sendablePressed;
}
void Trigger::WhenActive(Command* command) {
auto pbs = new PressedButtonScheduler(Grab(), this, command);
pbs->Start();
}
void Trigger::WhileActive(Command* command) {
auto hbs = new HeldButtonScheduler(Grab(), this, command);
hbs->Start();
}
void Trigger::WhenInactive(Command* command) {
auto rbs = new ReleasedButtonScheduler(Grab(), this, command);
rbs->Start();
}
void Trigger::CancelWhenActive(Command* command) {
auto cbs = new CancelButtonScheduler(Grab(), this, command);
cbs->Start();
}
void Trigger::ToggleWhenActive(Command* command) {
auto tbs = new ToggleButtonScheduler(Grab(), this, command);
tbs->Start();
}
void Trigger::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("Button");
builder.SetSafeState([=] { m_sendablePressed = false; });
builder.AddBooleanProperty(
"pressed", [=] { return Grab(); },
[=](bool value) { m_sendablePressed = value; });
}

315
wpilibOldCommands/src/main/native/cpp/commands/Command.cpp
View File

@@ -1,315 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/commands/Command.h"
#include <typeinfo>
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
#include "frc/Errors.h"
#include "frc/RobotState.h"
#include "frc/Timer.h"
#include "frc/commands/CommandGroup.h"
#include "frc/commands/Scheduler.h"
#include "frc/livewindow/LiveWindow.h"
using namespace frc;
int Command::m_commandCounter = 0;
Command::Command() : Command("", -1_s) {}
Command::Command(std::string_view name) : Command(name, -1_s) {}
Command::Command(units::second_t timeout) : Command("", timeout) {}
Command::Command(Subsystem& subsystem) : Command("", -1_s) {
Requires(&subsystem);
}
Command::Command(std::string_view name, units::second_t timeout) {
// We use -1.0 to indicate no timeout.
if (timeout < 0_s && timeout != -1_s) {
throw FRC_MakeError(err::ParameterOutOfRange, "timeout {} < 0 s",
timeout.value());
}
m_timeout = timeout;
// If name contains an empty string
if (name.empty()) {
wpi::SendableRegistry::Add(this,
fmt::format("Command_{}", typeid(*this).name()));
} else {
wpi::SendableRegistry::Add(this, name);
}
}
Command::Command(std::string_view name, Subsystem& subsystem)
: Command(name, -1_s) {
Requires(&subsystem);
}
Command::Command(units::second_t timeout, Subsystem& subsystem)
: Command("", timeout) {
Requires(&subsystem);
}
Command::Command(std::string_view name, units::second_t timeout,
Subsystem& subsystem)
: Command(name, timeout) {
Requires(&subsystem);
}
units::second_t Command::TimeSinceInitialized() const {
if (m_startTime < 0_s) {
return 0_s;
} else {
return Timer::GetFPGATimestamp() - m_startTime;
}
}
void Command::Requires(Subsystem* subsystem) {
if (!AssertUnlocked("Can not add new requirement to command")) {
return;
}
if (subsystem != nullptr) {
m_requirements.insert(subsystem);
} else {
throw FRC_MakeError(err::NullParameter, "{}", "subsystem");
}
}
void Command::Start() {
LockChanges();
if (m_parent != nullptr) {
throw FRC_MakeError(
err::CommandIllegalUse, "{}",
"Can not start a command that is part of a command group");
}
m_completed = false;
Scheduler::GetInstance()->AddCommand(this);
}
bool Command::Run() {
if (!m_runWhenDisabled && m_parent == nullptr && RobotState::IsDisabled()) {
Cancel();
}
if (IsCanceled()) {
return false;
}
if (!m_initialized) {
m_initialized = true;
StartTiming();
_Initialize();
Initialize();
}
_Execute();
Execute();
return !IsFinished();
}
void Command::Cancel() {
if (m_parent != nullptr) {
throw FRC_MakeError(
err::CommandIllegalUse, "{}",
"Can not cancel a command that is part of a command group");
}
_Cancel();
}
bool Command::IsRunning() const {
return m_running;
}
bool Command::IsInitialized() const {
return m_initialized;
}
bool Command::IsCompleted() const {
return m_completed;
}
bool Command::IsCanceled() const {
return m_canceled;
}
bool Command::IsInterruptible() const {
return m_interruptible;
}
void Command::SetInterruptible(bool interruptible) {
m_interruptible = interruptible;
}
bool Command::DoesRequire(Subsystem* system) const {
return m_requirements.count(system) > 0;
}
const Command::SubsystemSet& Command::GetRequirements() const {
return m_requirements;
}
CommandGroup* Command::GetGroup() const {
return m_parent;
}
void Command::SetRunWhenDisabled(bool run) {
m_runWhenDisabled = run;
}
bool Command::WillRunWhenDisabled() const {
return m_runWhenDisabled;
}
int Command::GetID() const {
return m_commandID;
}
void Command::SetTimeout(units::second_t timeout) {
if (timeout < 0_s) {
throw FRC_MakeError(err::ParameterOutOfRange, "timeout {} < 0 s",
timeout.value());
} else {
m_timeout = timeout;
}
}
bool Command::IsTimedOut() const {
return m_timeout != -1_s && TimeSinceInitialized() >= m_timeout;
}
bool Command::AssertUnlocked(std::string_view message) {
if (m_locked) {
throw FRC_MakeError(
err::CommandIllegalUse,
"{} after being started or being added to a command group", message);
}
return true;
}
void Command::SetParent(CommandGroup* parent) {
if (parent == nullptr) {
throw FRC_MakeError(err::NullParameter, "{}", "parent");
} else if (m_parent != nullptr) {
throw FRC_MakeError(err::CommandIllegalUse, "{}",
"Can not give command to a command group after "
"already being put in a command group");
} else {
LockChanges();
m_parent = parent;
}
}
bool Command::IsParented() const {
return m_parent != nullptr;
}
void Command::ClearRequirements() {
m_requirements.clear();
}
void Command::Initialize() {}
void Command::Execute() {}
void Command::End() {}
void Command::Interrupted() {
End();
}
void Command::_Initialize() {
m_completed = false;
}
void Command::_Interrupted() {
m_completed = true;
}
void Command::_Execute() {}
void Command::_End() {
m_completed = true;
}
void Command::_Cancel() {
if (IsRunning()) {
m_canceled = true;
}
}
void Command::LockChanges() {
m_locked = true;
}
void Command::Removed() {
if (m_initialized) {
if (IsCanceled()) {
Interrupted();
_Interrupted();
} else {
End();
_End();
}
}
m_initialized = false;
m_canceled = false;
m_running = false;
m_completed = true;
}
void Command::StartRunning() {
m_running = true;
m_startTime = -1_s;
m_completed = false;
}
void Command::StartTiming() {
m_startTime = Timer::GetFPGATimestamp();
}
std::string Command::GetName() const {
return wpi::SendableRegistry::GetName(this);
}
void Command::SetName(std::string_view name) {
wpi::SendableRegistry::SetName(this, name);
}
std::string Command::GetSubsystem() const {
return wpi::SendableRegistry::GetSubsystem(this);
}
void Command::SetSubsystem(std::string_view name) {
wpi::SendableRegistry::SetSubsystem(this, name);
}
void Command::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("Command");
builder.AddStringProperty(
".name", [=] { return wpi::SendableRegistry::GetName(this); }, nullptr);
builder.AddBooleanProperty(
"running", [=] { return IsRunning(); },
[=](bool value) {
if (value) {
if (!IsRunning()) {
Start();
}
} else {
if (IsRunning()) {
Cancel();
}
}
});
builder.AddBooleanProperty(
".isParented", [=] { return IsParented(); }, nullptr);
}

269
wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp
View File

@@ -1,269 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/commands/CommandGroup.h"
#include "frc/Errors.h"
using namespace frc;
CommandGroup::CommandGroup(std::string_view name) : Command(name) {}
void CommandGroup::AddSequential(Command* command) {
if (!command) {
throw FRC_MakeError(err::NullParameter, "{}", "command");
}
if (!AssertUnlocked("Cannot add new command to command group")) {
return;
}
m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence);
command->SetParent(this);
// Iterate through command->GetRequirements() and call Requires() on each
// required subsystem
for (auto&& requirement : command->GetRequirements()) {
Requires(requirement);
}
}
void CommandGroup::AddSequential(Command* command, units::second_t timeout) {
if (!command) {
throw FRC_MakeError(err::NullParameter, "{}", "command");
}
if (!AssertUnlocked("Cannot add new command to command group")) {
return;
}
if (timeout < 0_s) {
throw FRC_MakeError(err::ParameterOutOfRange, "timeout {} < 0 s",
timeout.value());
}
m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence,
timeout);
command->SetParent(this);
// Iterate through command->GetRequirements() and call Requires() on each
// required subsystem
for (auto&& requirement : command->GetRequirements()) {
Requires(requirement);
}
}
void CommandGroup::AddParallel(Command* command) {
if (!command) {
throw FRC_MakeError(err::NullParameter, "{}", "command");
return;
}
if (!AssertUnlocked("Cannot add new command to command group")) {
return;
}
m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild);
command->SetParent(this);
// Iterate through command->GetRequirements() and call Requires() on each
// required subsystem
for (auto&& requirement : command->GetRequirements()) {
Requires(requirement);
}
}
void CommandGroup::AddParallel(Command* command, units::second_t timeout) {
if (!command) {
throw FRC_MakeError(err::NullParameter, "{}", "command");
}
if (!AssertUnlocked("Cannot add new command to command group")) {
return;
}
if (timeout < 0_s) {
throw FRC_MakeError(err::ParameterOutOfRange, "timeout {} < 0 s",
timeout.value());
}
m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild,
timeout);
command->SetParent(this);
// Iterate through command->GetRequirements() and call Requires() on each
// required subsystem
for (auto&& requirement : command->GetRequirements()) {
Requires(requirement);
}
}
bool CommandGroup::IsInterruptible() const {
if (!Command::IsInterruptible()) {
return false;
}
if (m_currentCommandIndex != -1 &&
static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
Command* cmd = m_commands[m_currentCommandIndex].m_command;
if (!cmd->IsInterruptible()) {
return false;
}
}
for (const auto& child : m_children) {
if (!child->m_command->IsInterruptible()) {
return false;
}
}
return true;
}
int CommandGroup::GetSize() const {
return m_children.size();
}
void CommandGroup::Initialize() {}
void CommandGroup::Execute() {}
bool CommandGroup::IsFinished() {
return static_cast<size_t>(m_currentCommandIndex) >= m_commands.size() &&
m_children.empty();
}
void CommandGroup::End() {}
void CommandGroup::Interrupted() {}
void CommandGroup::_Initialize() {
m_currentCommandIndex = -1;
}
void CommandGroup::_Execute() {
CommandGroupEntry* entry;
Command* cmd = nullptr;
bool firstRun = false;
if (m_currentCommandIndex == -1) {
firstRun = true;
m_currentCommandIndex = 0;
}
// While there are still commands in this group to run
while (static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
// If a command is prepared to run
if (cmd != nullptr) {
// If command timed out, cancel it so it's removed from the Scheduler
if (entry->IsTimedOut()) {
cmd->_Cancel();
}
// If command finished or was canceled, remove it from Scheduler
if (cmd->Run()) {
break;
} else {
cmd->Removed();
// Advance to next command in group
m_currentCommandIndex++;
firstRun = true;
cmd = nullptr;
continue;
}
}
entry = &m_commands[m_currentCommandIndex];
cmd = nullptr;
switch (entry->m_state) {
case CommandGroupEntry::kSequence_InSequence:
cmd = entry->m_command;
if (firstRun) {
cmd->StartRunning();
CancelConflicts(cmd);
firstRun = false;
}
break;
case CommandGroupEntry::kSequence_BranchPeer:
// Start executing a parallel command and advance to next entry in group
m_currentCommandIndex++;
entry->m_command->Start();
break;
case CommandGroupEntry::kSequence_BranchChild:
m_currentCommandIndex++;
/* Causes scheduler to skip children of current command which require
* the same subsystems as it
*/
CancelConflicts(entry->m_command);
entry->m_command->StartRunning();
// Add current command entry to list of children of this group
m_children.push_back(entry);
break;
}
}
// Run Children
for (auto& entry : m_children) {
auto child = entry->m_command;
if (entry->IsTimedOut()) {
child->_Cancel();
}
// If child finished or was canceled, set it to nullptr. nullptr entries
// are removed later.
if (!child->Run()) {
child->Removed();
entry = nullptr;
}
}
m_children.erase(std::remove(m_children.begin(), m_children.end(), nullptr),
m_children.end());
}
void CommandGroup::_End() {
// Theoretically, we don't have to check this, but we do if teams override the
// IsFinished method
if (m_currentCommandIndex != -1 &&
static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
Command* cmd = m_commands[m_currentCommandIndex].m_command;
cmd->_Cancel();
cmd->Removed();
}
for (auto& child : m_children) {
Command* cmd = child->m_command;
cmd->_Cancel();
cmd->Removed();
}
m_children.clear();
}
void CommandGroup::_Interrupted() {
_End();
}
void CommandGroup::CancelConflicts(Command* command) {
for (auto childIter = m_children.begin(); childIter != m_children.end();) {
Command* child = (*childIter)->m_command;
bool erased = false;
for (auto&& requirement : command->GetRequirements()) {
if (child->DoesRequire(requirement)) {
child->_Cancel();
child->Removed();
childIter = m_children.erase(childIter);
erased = true;
break;
}
}
if (!erased) {
childIter++;
}
}
}

24
wpilibOldCommands/src/main/native/cpp/commands/CommandGroupEntry.cpp
View File

@@ -1,24 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/commands/CommandGroupEntry.h"
#include "frc/commands/Command.h"
using namespace frc;
CommandGroupEntry::CommandGroupEntry(Command* command, Sequence state,
units::second_t timeout)
: m_timeout(timeout), m_command(command), m_state(state) {}
bool CommandGroupEntry::IsTimedOut() const {
if (m_timeout < 0_s) {
return false;
}
auto time = m_command->TimeSinceInitialized();
if (time == 0_s) {
return false;
}
return time >= m_timeout;
}

79
wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp
View File

@@ -1,79 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/commands/ConditionalCommand.h"
#include "frc/commands/Scheduler.h"
using namespace frc;
static void RequireAll(Command& command, Command* onTrue, Command* onFalse) {
if (onTrue != nullptr) {
for (auto requirement : onTrue->GetRequirements()) {
command.Requires(requirement);
}
}
if (onFalse != nullptr) {
for (auto requirement : onFalse->GetRequirements()) {
command.Requires(requirement);
}
}
}
ConditionalCommand::ConditionalCommand(Command* onTrue, Command* onFalse) {
m_onTrue = onTrue;
m_onFalse = onFalse;
RequireAll(*this, onTrue, onFalse);
}
ConditionalCommand::ConditionalCommand(std::string_view name, Command* onTrue,
Command* onFalse)
: Command(name) {
m_onTrue = onTrue;
m_onFalse = onFalse;
RequireAll(*this, onTrue, onFalse);
}
void ConditionalCommand::_Initialize() {
if (Condition()) {
m_chosenCommand = m_onTrue;
} else {
m_chosenCommand = m_onFalse;
}
if (m_chosenCommand != nullptr) {
// This is a hack to make canceling the chosen command inside a
// CommandGroup work properly
m_chosenCommand->ClearRequirements();
m_chosenCommand->Start();
}
Command::_Initialize();
}
void ConditionalCommand::_Cancel() {
if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
m_chosenCommand->Cancel();
}
Command::_Cancel();
}
bool ConditionalCommand::IsFinished() {
if (m_chosenCommand != nullptr) {
return m_chosenCommand->IsCompleted();
} else {
return true;
}
}
void ConditionalCommand::_Interrupted() {
if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
m_chosenCommand->Cancel();
}
Command::_Interrupted();
}

47
wpilibOldCommands/src/main/native/cpp/commands/InstantCommand.cpp
View File

@@ -1,47 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/commands/InstantCommand.h"
#include <utility>
using namespace frc;
InstantCommand::InstantCommand(std::string_view name) : Command(name) {}
InstantCommand::InstantCommand(Subsystem& subsystem) : Command(subsystem) {}
InstantCommand::InstantCommand(std::string_view name, Subsystem& subsystem)
: Command(name, subsystem) {}
InstantCommand::InstantCommand(std::function<void()> func)
: m_func(std::move(func)) {}
InstantCommand::InstantCommand(Subsystem& subsystem, std::function<void()> func)
: InstantCommand(subsystem) {
m_func = func;
}
InstantCommand::InstantCommand(std::string_view name,
std::function<void()> func)
: InstantCommand(name) {
m_func = func;
}
InstantCommand::InstantCommand(std::string_view name, Subsystem& subsystem,
std::function<void()> func)
: InstantCommand(name, subsystem) {
m_func = func;
}
void InstantCommand::_Initialize() {
Command::_Initialize();
if (m_func) {
m_func();
}
}
bool InstantCommand::IsFinished() {
return true;
}

119
wpilibOldCommands/src/main/native/cpp/commands/PIDCommand.cpp
View File

@@ -1,119 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/commands/PIDCommand.h"
#include <wpi/sendable/SendableBuilder.h>
using namespace frc;
PIDCommand::PIDCommand(std::string_view name, double p, double i, double d,
double f, double period)
: Command(name) {
m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
}
PIDCommand::PIDCommand(double p, double i, double d, double f, double period) {
m_controller =
std::make_shared<PIDController>(p, i, d, f, this, this, period);
}
PIDCommand::PIDCommand(std::string_view name, double p, double i, double d)
: Command(name) {
m_controller = std::make_shared<PIDController>(p, i, d, this, this);
}
PIDCommand::PIDCommand(std::string_view name, double p, double i, double d,
double period)
: Command(name) {
m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
}
PIDCommand::PIDCommand(double p, double i, double d) {
m_controller = std::make_shared<PIDController>(p, i, d, this, this);
}
PIDCommand::PIDCommand(double p, double i, double d, double period) {
m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
}
PIDCommand::PIDCommand(std::string_view name, double p, double i, double d,
double f, double period, Subsystem& subsystem)
: Command(name, subsystem) {
m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
}
PIDCommand::PIDCommand(double p, double i, double d, double f, double period,
Subsystem& subsystem)
: Command(subsystem) {
m_controller =
std::make_shared<PIDController>(p, i, d, f, this, this, period);
}
PIDCommand::PIDCommand(std::string_view name, double p, double i, double d,
Subsystem& subsystem)
: Command(name, subsystem) {
m_controller = std::make_shared<PIDController>(p, i, d, this, this);
}
PIDCommand::PIDCommand(std::string_view name, double p, double i, double d,
double period, Subsystem& subsystem)
: Command(name, subsystem) {
m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
}
PIDCommand::PIDCommand(double p, double i, double d, Subsystem& subsystem) {
m_controller = std::make_shared<PIDController>(p, i, d, this, this);
}
PIDCommand::PIDCommand(double p, double i, double d, double period,
Subsystem& subsystem) {
m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
}
void PIDCommand::_Initialize() {
m_controller->Enable();
}
void PIDCommand::_End() {
m_controller->Disable();
}
void PIDCommand::_Interrupted() {
_End();
}
void PIDCommand::SetSetpointRelative(double deltaSetpoint) {
SetSetpoint(GetSetpoint() + deltaSetpoint);
}
void PIDCommand::PIDWrite(double output) {
UsePIDOutput(output);
}
double PIDCommand::PIDGet() {
return ReturnPIDInput();
}
std::shared_ptr<PIDController> PIDCommand::GetPIDController() const {
return m_controller;
}
void PIDCommand::SetSetpoint(double setpoint) {
m_controller->SetSetpoint(setpoint);
}
double PIDCommand::GetSetpoint() const {
return m_controller->GetSetpoint();
}
double PIDCommand::GetPosition() {
return ReturnPIDInput();
}
void PIDCommand::InitSendable(wpi::SendableBuilder& builder) {
m_controller->InitSendable(builder);
Command::InitSendable(builder);
builder.SetSmartDashboardType("PIDCommand");
}

110
wpilibOldCommands/src/main/native/cpp/commands/PIDSubsystem.cpp
View File

@@ -1,110 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/commands/PIDSubsystem.h"
#include "frc/PIDController.h"
using namespace frc;
PIDSubsystem::PIDSubsystem(std::string_view name, double p, double i, double d)
: Subsystem(name) {
m_controller = std::make_shared<PIDController>(p, i, d, this, this);
AddChild("PIDController", m_controller);
}
PIDSubsystem::PIDSubsystem(std::string_view name, double p, double i, double d,
double f)
: Subsystem(name) {
m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
AddChild("PIDController", m_controller);
}
PIDSubsystem::PIDSubsystem(std::string_view name, double p, double i, double d,
double f, double period)
: Subsystem(name) {
m_controller =
std::make_shared<PIDController>(p, i, d, f, this, this, period);
AddChild("PIDController", m_controller);
}
PIDSubsystem::PIDSubsystem(double p, double i, double d)
: Subsystem("PIDSubsystem") {
m_controller = std::make_shared<PIDController>(p, i, d, this, this);
AddChild("PIDController", m_controller);
}
PIDSubsystem::PIDSubsystem(double p, double i, double d, double f)
: Subsystem("PIDSubsystem") {
m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
AddChild("PIDController", m_controller);
}
PIDSubsystem::PIDSubsystem(double p, double i, double d, double f,
double period)
: Subsystem("PIDSubsystem") {
m_controller =
std::make_shared<PIDController>(p, i, d, f, this, this, period);
AddChild("PIDController", m_controller);
}
void PIDSubsystem::Enable() {
m_controller->Enable();
}
void PIDSubsystem::Disable() {
m_controller->Disable();
}
void PIDSubsystem::PIDWrite(double output) {
UsePIDOutput(output);
}
double PIDSubsystem::PIDGet() {
return ReturnPIDInput();
}
void PIDSubsystem::SetSetpoint(double setpoint) {
m_controller->SetSetpoint(setpoint);
}
void PIDSubsystem::SetSetpointRelative(double deltaSetpoint) {
SetSetpoint(GetSetpoint() + deltaSetpoint);
}
void PIDSubsystem::SetInputRange(double minimumInput, double maximumInput) {
m_controller->SetInputRange(minimumInput, maximumInput);
}
void PIDSubsystem::SetOutputRange(double minimumOutput, double maximumOutput) {
m_controller->SetOutputRange(minimumOutput, maximumOutput);
}
double PIDSubsystem::GetSetpoint() {
return m_controller->GetSetpoint();
}
double PIDSubsystem::GetPosition() {
return ReturnPIDInput();
}
double PIDSubsystem::GetRate() {
return ReturnPIDInput();
}
void PIDSubsystem::SetAbsoluteTolerance(double absValue) {
m_controller->SetAbsoluteTolerance(absValue);
}
void PIDSubsystem::SetPercentTolerance(double percent) {
m_controller->SetPercentTolerance(percent);
}
bool PIDSubsystem::OnTarget() const {
return m_controller->OnTarget();
}
std::shared_ptr<PIDController> PIDSubsystem::GetPIDController() {
return m_controller;
}

18
wpilibOldCommands/src/main/native/cpp/commands/PrintCommand.cpp
View File

@@ -1,18 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/commands/PrintCommand.h"
#include <fmt/format.h>
using namespace frc;
PrintCommand::PrintCommand(std::string_view message)
: InstantCommand(fmt::format("Print \"{}\"", message)) {
m_message = message;
}
void PrintCommand::Initialize() {
fmt::print("{}\n", m_message);
}

262
wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp
View File

@@ -1,262 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/commands/Scheduler.h"
#include <algorithm>
#include <set>
#include <string>
#include <vector>
#include <hal/FRCUsageReporting.h>
#include <networktables/NTSendableBuilder.h>
#include <networktables/NetworkTableEntry.h>
#include <wpi/mutex.h>
#include <wpi/sendable/SendableRegistry.h>
#include "frc/Errors.h"
#include "frc/buttons/ButtonScheduler.h"
#include "frc/commands/Command.h"
#include "frc/commands/Subsystem.h"
#include "frc/livewindow/LiveWindow.h"
using namespace frc;
struct Scheduler::Impl {
void Remove(Command* command);
void ProcessCommandAddition(Command* command);
using SubsystemSet = std::set<Subsystem*>;
SubsystemSet subsystems;
wpi::mutex buttonsMutex;
using ButtonVector = std::vector<std::unique_ptr<ButtonScheduler>>;
ButtonVector buttons;
using CommandVector = std::vector<Command*>;
wpi::mutex additionsMutex;
CommandVector additions;
using CommandSet = std::set<Command*>;
CommandSet commands;
bool adding = false;
bool enabled = true;
std::vector<std::string> commandsBuf;
std::vector<double> idsBuf;
bool runningCommandsChanged = false;
};
Scheduler* Scheduler::GetInstance() {
static Scheduler instance;
return &instance;
}
void Scheduler::AddCommand(Command* command) {
std::scoped_lock lock(m_impl->additionsMutex);
if (std::find(m_impl->additions.begin(), m_impl->additions.end(), command) !=
m_impl->additions.end()) {
return;
}
m_impl->additions.push_back(command);
}
void Scheduler::AddButton(ButtonScheduler* button) {
std::scoped_lock lock(m_impl->buttonsMutex);
m_impl->buttons.emplace_back(button);
}
void Scheduler::RegisterSubsystem(Subsystem* subsystem) {
if (!subsystem) {
throw FRC_MakeError(err::NullParameter, "{}", "subsystem");
}
m_impl->subsystems.insert(subsystem);
}
void Scheduler::Run() {
// Get button input (going backwards preserves button priority)
{
if (!m_impl->enabled) {
return;
}
std::scoped_lock lock(m_impl->buttonsMutex);
for (auto& button : m_impl->buttons) {
button->Execute();
}
}
// Call every subsystem's periodic method
for (auto& subsystem : m_impl->subsystems) {
subsystem->Periodic();
}
m_impl->runningCommandsChanged = false;
// Loop through the commands
for (auto cmdIter = m_impl->commands.begin();
cmdIter != m_impl->commands.end();) {
Command* command = *cmdIter;
// Increment before potentially removing to keep the iterator valid
++cmdIter;
if (!command->Run()) {
Remove(command);
m_impl->runningCommandsChanged = true;
}
}
// Add the new things
{
std::scoped_lock lock(m_impl->additionsMutex);
for (auto& addition : m_impl->additions) {
// Check to make sure no adding during adding
if (m_impl->adding) {
FRC_ReportError(warn::IncompatibleState, "{}",
"Can not start command from cancel method");
} else {
m_impl->ProcessCommandAddition(addition);
}
}
m_impl->additions.clear();
}
// Add in the defaults
for (auto& subsystem : m_impl->subsystems) {
if (subsystem->GetCurrentCommand() == nullptr) {
if (m_impl->adding) {
FRC_ReportError(warn::IncompatibleState, "{}",
"Can not start command from cancel method");
} else {
m_impl->ProcessCommandAddition(subsystem->GetDefaultCommand());
}
}
subsystem->ConfirmCommand();
}
}
void Scheduler::Remove(Command* command) {
if (!command) {
throw FRC_MakeError(err::NullParameter, "{}", "command");
}
m_impl->Remove(command);
}
void Scheduler::RemoveAll() {
while (m_impl->commands.size() > 0) {
Remove(*m_impl->commands.begin());
}
}
void Scheduler::ResetAll() {
RemoveAll();
m_impl->subsystems.clear();
m_impl->buttons.clear();
m_impl->additions.clear();
m_impl->commands.clear();
}
void Scheduler::SetEnabled(bool enabled) {
m_impl->enabled = enabled;
}
void Scheduler::InitSendable(nt::NTSendableBuilder& builder) {
builder.SetSmartDashboardType("Scheduler");
auto namesEntry = builder.GetEntry("Names");
auto idsEntry = builder.GetEntry("Ids");
auto cancelEntry = builder.GetEntry("Cancel");
builder.SetUpdateTable([=] {
// Get the list of possible commands to cancel
auto new_toCancel = cancelEntry.GetValue();
wpi::span<const double> toCancel;
if (new_toCancel) {
toCancel = new_toCancel->GetDoubleArray();
}
// Cancel commands whose cancel buttons were pressed on the SmartDashboard
if (!toCancel.empty()) {
for (auto& command : m_impl->commands) {
for (const auto& canceled : toCancel) {
if (command->GetID() == canceled) {
command->Cancel();
}
}
}
nt::NetworkTableEntry(cancelEntry).SetDoubleArray({});
}
// Set the running commands
if (m_impl->runningCommandsChanged) {
m_impl->commandsBuf.resize(0);
m_impl->idsBuf.resize(0);
for (const auto& command : m_impl->commands) {
m_impl->commandsBuf.emplace_back(
wpi::SendableRegistry::GetName(command));
m_impl->idsBuf.emplace_back(command->GetID());
}
nt::NetworkTableEntry(namesEntry).SetStringArray(m_impl->commandsBuf);
nt::NetworkTableEntry(idsEntry).SetDoubleArray(m_impl->idsBuf);
}
});
}
Scheduler::Scheduler() : m_impl(new Impl) {
HAL_Report(HALUsageReporting::kResourceType_Command,
HALUsageReporting::kCommand_Scheduler);
wpi::SendableRegistry::AddLW(this, "Scheduler");
frc::LiveWindow::SetEnabledCallback([this] {
this->SetEnabled(false);
this->RemoveAll();
});
frc::LiveWindow::SetDisabledCallback([this] { this->SetEnabled(true); });
}
Scheduler::~Scheduler() {
wpi::SendableRegistry::Remove(this);
frc::LiveWindow::SetEnabledCallback(nullptr);
frc::LiveWindow::SetDisabledCallback(nullptr);
}
void Scheduler::Impl::Remove(Command* command) {
if (!commands.erase(command)) {
return;
}
for (auto&& requirement : command->GetRequirements()) {
requirement->SetCurrentCommand(nullptr);
}
command->Removed();
}
void Scheduler::Impl::ProcessCommandAddition(Command* command) {
if (command == nullptr) {
return;
}
// Only add if not already in
auto found = commands.find(command);
if (found == commands.end()) {
// Check that the requirements can be had
const auto& requirements = command->GetRequirements();
for (const auto requirement : requirements) {
if (requirement->GetCurrentCommand() != nullptr &&
!requirement->GetCurrentCommand()->IsInterruptible()) {
return;
}
}
// Give it the requirements
adding = true;
for (auto&& requirement : requirements) {
if (requirement->GetCurrentCommand() != nullptr) {
requirement->GetCurrentCommand()->Cancel();
Remove(requirement->GetCurrentCommand());
}
requirement->SetCurrentCommand(command);
}
adding = false;
commands.insert(command);
command->StartRunning();
runningCommandsChanged = true;
}
}

16
wpilibOldCommands/src/main/native/cpp/commands/StartCommand.cpp
View File

@@ -1,16 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/commands/StartCommand.h"
using namespace frc;
StartCommand::StartCommand(Command* commandToStart)
: InstantCommand("StartCommand") {
m_commandToFork = commandToStart;
}
void StartCommand::Initialize() {
m_commandToFork->Start();
}

137
wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp
View File

@@ -1,137 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/commands/Subsystem.h"
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
#include "frc/Errors.h"
#include "frc/commands/Command.h"
#include "frc/commands/Scheduler.h"
#include "frc/livewindow/LiveWindow.h"
using namespace frc;
Subsystem::Subsystem(std::string_view name) {
wpi::SendableRegistry::AddLW(this, name, name);
Scheduler::GetInstance()->RegisterSubsystem(this);
}
void Subsystem::SetDefaultCommand(Command* command) {
if (command == nullptr) {
m_defaultCommand = nullptr;
} else {
const auto& reqs = command->GetRequirements();
if (std::find(reqs.begin(), reqs.end(), this) == reqs.end()) {
throw FRC_MakeError(err::CommandIllegalUse, "{}",
"A default command must require the subsystem");
}
m_defaultCommand = command;
}
}
Command* Subsystem::GetDefaultCommand() {
if (!m_initializedDefaultCommand) {
m_initializedDefaultCommand = true;
InitDefaultCommand();
}
return m_defaultCommand;
}
std::string Subsystem::GetDefaultCommandName() {
Command* defaultCommand = GetDefaultCommand();
if (defaultCommand) {
return wpi::SendableRegistry::GetName(defaultCommand);
} else {
return {};
}
}
void Subsystem::SetCurrentCommand(Command* command) {
m_currentCommand = command;
m_currentCommandChanged = true;
}
Command* Subsystem::GetCurrentCommand() const {
return m_currentCommand;
}
std::string Subsystem::GetCurrentCommandName() const {
Command* currentCommand = GetCurrentCommand();
if (currentCommand) {
return wpi::SendableRegistry::GetName(currentCommand);
} else {
return {};
}
}
void Subsystem::Periodic() {}
void Subsystem::InitDefaultCommand() {}
std::string Subsystem::GetName() const {
return wpi::SendableRegistry::GetName(this);
}
void Subsystem::SetName(std::string_view name) {
wpi::SendableRegistry::SetName(this, name);
}
std::string Subsystem::GetSubsystem() const {
return wpi::SendableRegistry::GetSubsystem(this);
}
void Subsystem::SetSubsystem(std::string_view name) {
wpi::SendableRegistry::SetSubsystem(this, name);
}
void Subsystem::AddChild(std::string_view name,
std::shared_ptr<Sendable> child) {
AddChild(name, *child);
}
void Subsystem::AddChild(std::string_view name, wpi::Sendable* child) {
AddChild(name, *child);
}
void Subsystem::AddChild(std::string_view name, wpi::Sendable& child) {
wpi::SendableRegistry::AddLW(&child,
wpi::SendableRegistry::GetSubsystem(this), name);
}
void Subsystem::AddChild(std::shared_ptr<wpi::Sendable> child) {
AddChild(*child);
}
void Subsystem::AddChild(wpi::Sendable* child) {
AddChild(*child);
}
void Subsystem::AddChild(wpi::Sendable& child) {
wpi::SendableRegistry::SetSubsystem(
&child, wpi::SendableRegistry::GetSubsystem(this));
wpi::SendableRegistry::EnableLiveWindow(&child);
}
void Subsystem::ConfirmCommand() {
if (m_currentCommandChanged) {
m_currentCommandChanged = false;
}
}
void Subsystem::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("Subsystem");
builder.AddBooleanProperty(
".hasDefault", [=] { return m_defaultCommand != nullptr; }, nullptr);
builder.AddStringProperty(
".default", [=] { return GetDefaultCommandName(); }, nullptr);
builder.AddBooleanProperty(
".hasCommand", [=] { return m_currentCommand != nullptr; }, nullptr);
builder.AddStringProperty(
".command", [=] { return GetCurrentCommandName(); }, nullptr);
}

23
wpilibOldCommands/src/main/native/cpp/commands/TimedCommand.cpp
View File

@@ -1,23 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/commands/TimedCommand.h"
using namespace frc;
TimedCommand::TimedCommand(std::string_view name, units::second_t timeout)
: Command(name, timeout) {}
TimedCommand::TimedCommand(units::second_t timeout) : Command(timeout) {}
TimedCommand::TimedCommand(std::string_view name, units::second_t timeout,
Subsystem& subsystem)
: Command(name, timeout, subsystem) {}
TimedCommand::TimedCommand(units::second_t timeout, Subsystem& subsystem)
: Command(timeout, subsystem) {}
bool TimedCommand::IsFinished() {
return IsTimedOut();
}

15
wpilibOldCommands/src/main/native/cpp/commands/WaitCommand.cpp
View File

@@ -1,15 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/commands/WaitCommand.h"
#include <fmt/format.h>
using namespace frc;
WaitCommand::WaitCommand(units::second_t timeout)
: TimedCommand(fmt::format("Wait({})", timeout.value()), timeout) {}
WaitCommand::WaitCommand(std::string_view name, units::second_t timeout)
: TimedCommand(name, timeout) {}

19
wpilibOldCommands/src/main/native/cpp/commands/WaitForChildren.cpp
View File

@@ -1,19 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/commands/WaitForChildren.h"
#include "frc/commands/CommandGroup.h"
using namespace frc;
WaitForChildren::WaitForChildren(units::second_t timeout)
: Command("WaitForChildren", timeout) {}
WaitForChildren::WaitForChildren(std::string_view name, units::second_t timeout)
: Command(name, timeout) {}
bool WaitForChildren::IsFinished() {
return GetGroup() == nullptr || GetGroup()->GetSize() == 0;
}

23
wpilibOldCommands/src/main/native/cpp/commands/WaitUntilCommand.cpp
View File

@@ -1,23 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/commands/WaitUntilCommand.h"
#include "frc/Timer.h"
using namespace frc;
WaitUntilCommand::WaitUntilCommand(units::second_t time)
: Command("WaitUntilCommand", time) {
m_time = time;
}
WaitUntilCommand::WaitUntilCommand(std::string_view name, units::second_t time)
: Command(name, time) {
m_time = time;
}
bool WaitUntilCommand::IsFinished() {
return Timer::GetMatchTime() >= m_time;
}

11
wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogAccelerometer.cpp
View File

@@ -1,11 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/pidwrappers/PIDAnalogAccelerometer.h"
using namespace frc;
double PIDAnalogAccelerometer::PIDGet() {
return GetAcceleration();
}

18
wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogGyro.cpp
View File

@@ -1,18 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/pidwrappers/PIDAnalogGyro.h"
using namespace frc;
double PIDAnalogGyro::PIDGet() {
switch (GetPIDSourceType()) {
case PIDSourceType::kRate:
return GetRate();
case PIDSourceType::kDisplacement:
return GetAngle();
default:
return 0;
}
}

11
wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogInput.cpp
View File

@@ -1,11 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/pidwrappers/PIDAnalogInput.h"
using namespace frc;
double PIDAnalogInput::PIDGet() {
return GetAverageVoltage();
}

11
wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogPotentiometer.cpp
View File

@@ -1,11 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/pidwrappers/PIDAnalogPotentiometer.h"
using namespace frc;
double PIDAnalogPotentiometer::PIDGet() {
return Get();
}

18
wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDEncoder.cpp
View File

@@ -1,18 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/pidwrappers/PIDEncoder.h"
using namespace frc;
double PIDEncoder::PIDGet() {
switch (GetPIDSourceType()) {
case PIDSourceType::kDisplacement:
return GetDistance();
case PIDSourceType::kRate:
return GetRate();
default:
return 0.0;
}
}

50
wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDMotorController.cpp
View File

@@ -1,50 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/pidwrappers/PIDMotorController.h"
#include <fmt/format.h>
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
using namespace frc;
PIDMotorController::PIDMotorController(MotorController& motorController)
: m_motorController(motorController) {}
void PIDMotorController::Set(double speed) {
m_motorController.Set(m_isInverted ? -speed : speed);
}
double PIDMotorController::Get() const {
return m_motorController.Get() * (m_isInverted ? -1.0 : 1.0);
}
void PIDMotorController::SetInverted(bool isInverted) {
m_isInverted = isInverted;
}
bool PIDMotorController::GetInverted() const {
return m_isInverted;
}
void PIDMotorController::Disable() {
m_motorController.Disable();
}
void PIDMotorController::StopMotor() {
Disable();
}
void PIDMotorController::PIDWrite(double output) {
m_motorController.Set(output);
}
void PIDMotorController::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("Motor Controller");
builder.SetActuator(true);
builder.SetSafeState([=] { Disable(); });
builder.AddDoubleProperty(
"Value", [=] { return Get(); }, [=](double value) { Set(value); });
}

11
wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDUltrasonic.cpp
View File

@@ -1,11 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/pidwrappers/PIDUltrasonic.h"
using namespace frc;
double PIDUltrasonic::PIDGet() {
return GetRange().value();
}

407
wpilibOldCommands/src/main/native/include/frc/PIDBase.h
View File

@@ -1,407 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <memory>
#include <string>
#include <wpi/deprecated.h>
#include <wpi/mutex.h>
#include <wpi/sendable/Sendable.h>
#include <wpi/sendable/SendableHelper.h>
#include "frc/PIDInterface.h"
#include "frc/PIDOutput.h"
#include "frc/PIDSource.h"
#include "frc/Timer.h"
#include "frc/filter/LinearFilter.h"
namespace frc {
/**
* Class implements a PID Control Loop.
*
* Creates a separate thread which reads the given PIDSource and takes care of
* the integral calculations, as well as writing the given PIDOutput.
*
* This feedback controller runs in discrete time, so time deltas are not used
* in the integral and derivative calculations. Therefore, the sample rate
* affects the controller's behavior for a given set of PID constants.
*
* This class is provided by the OldCommands VendorDep
*
* @deprecated All APIs which use this have been deprecated.
*/
class PIDBase : public PIDInterface,
public PIDOutput,
public wpi::Sendable,
public wpi::SendableHelper<PIDBase> {
public:
/**
* Allocate a PID object with the given constants for P, I, D.
*
* @param p the proportional coefficient
* @param i the integral coefficient
* @param d the derivative coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
*/
WPI_DEPRECATED("All APIs which use this have been deprecated.")
PIDBase(double p, double i, double d, PIDSource& source, PIDOutput& output);
/**
* Allocate a PID object with the given constants for P, I, D.
*
* @param p the proportional coefficient
* @param i the integral coefficient
* @param d the derivative coefficient
* @param f the feedforward coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
*/
WPI_DEPRECATED("All APIs which use this have been deprecated.")
PIDBase(double p, double i, double d, double f, PIDSource& source,
PIDOutput& output);
~PIDBase() override = default;
/**
* Return the current PID result.
*
* This is always centered on zero and constrained the the max and min outs.
*
* @return the latest calculated output
*/
virtual double Get() const;
/**
* Set the PID controller to consider the input to be continuous,
*
* Rather then using the max and min input range as constraints, it considers
* them to be the same point and automatically calculates the shortest route
* to the setpoint.
*
* @param continuous true turns on continuous, false turns off continuous
*/
virtual void SetContinuous(bool continuous = true);
/**
* Sets the maximum and minimum values expected from the input.
*
* @param minimumInput the minimum value expected from the input
* @param maximumInput the maximum value expected from the output
*/
virtual void SetInputRange(double minimumInput, double maximumInput);
/**
* Sets the minimum and maximum values to write.
*
* @param minimumOutput the minimum value to write to the output
* @param maximumOutput the maximum value to write to the output
*/
virtual void SetOutputRange(double minimumOutput, double maximumOutput);
/**
* Set the PID Controller gain parameters.
*
* Set the proportional, integral, and differential coefficients.
*
* @param p Proportional coefficient
* @param i Integral coefficient
* @param d Differential coefficient
*/
void SetPID(double p, double i, double d) override;
/**
* Set the PID Controller gain parameters.
*
* Set the proportional, integral, and differential coefficients.
*
* @param p Proportional coefficient
* @param i Integral coefficient
* @param d Differential coefficient
* @param f Feed forward coefficient
*/
virtual void SetPID(double p, double i, double d, double f);
/**
* Set the Proportional coefficient of the PID controller gain.
*
* @param p proportional coefficient
*/
void SetP(double p);
/**
* Set the Integral coefficient of the PID controller gain.
*
* @param i integral coefficient
*/
void SetI(double i);
/**
* Set the Differential coefficient of the PID controller gain.
*
* @param d differential coefficient
*/
void SetD(double d);
/**
* Get the Feed forward coefficient of the PID controller gain.
*
* @param f Feed forward coefficient
*/
void SetF(double f);
/**
* Get the Proportional coefficient.
*
* @return proportional coefficient
*/
double GetP() const override;
/**
* Get the Integral coefficient.
*
* @return integral coefficient
*/
double GetI() const override;
/**
* Get the Differential coefficient.
*
* @return differential coefficient
*/
double GetD() const override;
/**
* Get the Feed forward coefficient.
*
* @return Feed forward coefficient
*/
virtual double GetF() const;
/**
* Set the setpoint for the PIDBase.
*
* @param setpoint the desired setpoint
*/
void SetSetpoint(double setpoint) override;
/**
* Returns the current setpoint of the PIDBase.
*
* @return the current setpoint
*/
double GetSetpoint() const override;
/**
* Returns the change in setpoint over time of the PIDBase.
*
* @return the change in setpoint over time
*/
double GetDeltaSetpoint() const;
/**
* Returns the current difference of the input from the setpoint.
*
* @return the current error
*/
virtual double GetError() const;
/**
* Returns the current average of the error over the past few iterations.
*
* You can specify the number of iterations to average with
* SetToleranceBuffer() (defaults to 1). This is the same value that is used
* for OnTarget().
*
* @return the average error
*/
WPI_DEPRECATED("Use a LinearFilter as the input and GetError().")
virtual double GetAvgError() const;
/**
* Sets what type of input the PID controller will use.
*/
virtual void SetPIDSourceType(PIDSourceType pidSource);
/**
* Returns the type of input the PID controller is using.
*
* @return the PID controller input type
*/
virtual PIDSourceType GetPIDSourceType() const;
/**
* Set the percentage error which is considered tolerable for use with
* OnTarget.
*
* @param percent error which is tolerable
*/
WPI_DEPRECATED("Use SetPercentTolerance() instead.")
virtual void SetTolerance(double percent);
/**
* Set the absolute error which is considered tolerable for use with
* OnTarget.
*
* @param absTolerance error which is tolerable
*/
virtual void SetAbsoluteTolerance(double absTolerance);
/**
* Set the percentage error which is considered tolerable for use with
* OnTarget.
*
* @param percent error which is tolerable
*/
virtual void SetPercentTolerance(double percent);
/**
* Set the number of previous error samples to average for tolerancing. When
* determining whether a mechanism is on target, the user may want to use a
* rolling average of previous measurements instead of a precise position or
* velocity. This is useful for noisy sensors which return a few erroneous
* measurements when the mechanism is on target. However, the mechanism will
* not register as on target for at least the specified bufLength cycles.
*
* @param bufLength Number of previous cycles to average. Defaults to 1.
*/
WPI_DEPRECATED("Use a LinearDigitalFilter as the input.")
virtual void SetToleranceBuffer(int bufLength = 1);
/**
* Return true if the error is within the percentage of the total input range,
* determined by SetTolerance. This asssumes that the maximum and minimum
* input were set using SetInput.
*
* Currently this just reports on target as the actual value passes through
* the setpoint. Ideally it should be based on being within the tolerance for
* some period of time.
*
* This will return false until at least one input value has been computed.
*/
virtual bool OnTarget() const;
/**
* Reset the previous error, the integral term, and disable the controller.
*/
void Reset() override;
/**
* Passes the output directly to SetSetpoint().
*
* PIDControllers can be nested by passing a PIDController as another
* PIDController's output. In that case, the output of the parent controller
* becomes the input (i.e., the reference) of the child.
*
* It is the caller's responsibility to put the data into a valid form for
* SetSetpoint().
*/
void PIDWrite(double output) override;
void InitSendable(wpi::SendableBuilder& builder) override;
protected:
// Is the pid controller enabled
bool m_enabled = false;
mutable wpi::mutex m_thisMutex;
// Ensures when Disable() is called, PIDWrite() won't run if Calculate()
// is already running at that time.
mutable wpi::mutex m_pidWriteMutex;
PIDSource* m_pidInput;
PIDOutput* m_pidOutput;
Timer m_setpointTimer;
/**
* Read the input, calculate the output accordingly, and write to the output.
* This should only be called by the Notifier.
*/
virtual void Calculate();
/**
* Calculate the feed forward term.
*
* Both of the provided feed forward calculations are velocity feed forwards.
* If a different feed forward calculation is desired, the user can override
* this function and provide his or her own. This function does no
* synchronization because the PIDBase class only calls it in synchronized
* code, so be careful if calling it oneself.
*
* If a velocity PID controller is being used, the F term should be set to 1
* over the maximum setpoint for the output. If a position PID controller is
* being used, the F term should be set to 1 over the maximum speed for the
* output measured in setpoint units per this controller's update period (see
* the default period in this class's constructor).
*/
virtual double CalculateFeedForward();
/**
* Wraps error around for continuous inputs. The original error is returned if
* continuous mode is disabled. This is an unsynchronized function.
*
* @param error The current error of the PID controller.
* @return Error for continuous inputs.
*/
double GetContinuousError(double error) const;
private:
// Factor for "proportional" control
double m_P;
// Factor for "integral" control
double m_I;
// Factor for "derivative" control
double m_D;
// Factor for "feed forward" control
double m_F;
// |maximum output|
double m_maximumOutput = 1.0;
// |minimum output|
double m_minimumOutput = -1.0;
// Maximum input - limit setpoint to this
double m_maximumInput = 0;
// Minimum input - limit setpoint to this
double m_minimumInput = 0;
// input range - difference between maximum and minimum
double m_inputRange = 0;
// Do the endpoints wrap around? eg. Absolute encoder
bool m_continuous = false;
// The prior error (used to compute velocity)
double m_prevError = 0;
// The sum of the errors for use in the integral calc
double m_totalError = 0;
enum {
kAbsoluteTolerance,
kPercentTolerance,
kNoTolerance
} m_toleranceType = kNoTolerance;
// The percetage or absolute error that is considered on target.
double m_tolerance = 0.05;
double m_setpoint = 0;
double m_prevSetpoint = 0;
double m_error = 0;
double m_result = 0;
LinearFilter<double> m_filter{{}, {}};
};
} // namespace frc

139
wpilibOldCommands/src/main/native/include/frc/PIDController.h
View File

@@ -1,139 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <memory>
#include <string>
#include <wpi/deprecated.h>
#include <wpi/mutex.h>
#include "frc/Controller.h"
#include "frc/Notifier.h"
#include "frc/PIDBase.h"
#include "frc/PIDSource.h"
#include "frc/Timer.h"
namespace frc {
class PIDOutput;
/**
* Class implements a PID Control Loop.
*
* Creates a separate thread which reads the given PIDSource and takes care of
* the integral calculations, as well as writing the given PIDOutput.
*
* This feedback controller runs in discrete time, so time deltas are not used
* in the integral and derivative calculations. Therefore, the sample rate
* affects the controller's behavior for a given set of PID constants.
*
* This class is provided by the OldCommands VendorDep
*
* @deprecated Use frc2::PIDController class instead.
*/
class PIDController : public PIDBase, public Controller {
public:
/**
* Allocate a PID object with the given constants for P, I, D.
*
* @param p the proportional coefficient
* @param i the integral coefficient
* @param d the derivative coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
* @param period the loop time for doing calculations in seconds. This
* particularly affects calculations of the integral and
* differential terms. The default is 0.05 (50ms).
*/
WPI_DEPRECATED("Use frc2::PIDController class instead.")
PIDController(double p, double i, double d, PIDSource* source,
PIDOutput* output, double period = 0.05);
/**
* Allocate a PID object with the given constants for P, I, D.
*
* @param p the proportional coefficient
* @param i the integral coefficient
* @param d the derivative coefficient
* @param f the feedforward coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
* @param period the loop time for doing calculations in seconds. This
* particularly affects calculations of the integral and
* differential terms. The default is 0.05 (50ms).
*/
WPI_DEPRECATED("Use frc2::PIDController class instead.")
PIDController(double p, double i, double d, double f, PIDSource* source,
PIDOutput* output, double period = 0.05);
/**
* Allocate a PID object with the given constants for P, I, D.
*
* @param p the proportional coefficient
* @param i the integral coefficient
* @param d the derivative coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
* @param period the loop time for doing calculations in seconds. This
* particularly affects calculations of the integral and
* differential terms. The default is 0.05 (50ms).
*/
WPI_DEPRECATED("Use frc2::PIDController class instead.")
PIDController(double p, double i, double d, PIDSource& source,
PIDOutput& output, double period = 0.05);
/**
* Allocate a PID object with the given constants for P, I, D.
*
* @param p the proportional coefficient
* @param i the integral coefficient
* @param d the derivative coefficient
* @param f the feedforward coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
* @param period the loop time for doing calculations in seconds. This
* particularly affects calculations of the integral and
* differential terms. The default is 0.05 (50ms).
*/
WPI_DEPRECATED("Use frc2::PIDController class instead.")
PIDController(double p, double i, double d, double f, PIDSource& source,
PIDOutput& output, double period = 0.05);
~PIDController() override;
/**
* Begin running the PIDController.
*/
void Enable() override;
/**
* Stop running the PIDController, this sets the output to zero before
* stopping.
*/
void Disable() override;
/**
* Set the enabled state of the PIDController.
*/
void SetEnabled(bool enable);
/**
* Return true if PIDController is enabled.
*/
bool IsEnabled() const;
/**
* Reset the previous error, the integral term, and disable the controller.
*/
void Reset() override;
void InitSendable(wpi::SendableBuilder& builder) override;
private:
std::unique_ptr<Notifier> m_controlLoop;
};
} // namespace frc

36
wpilibOldCommands/src/main/native/include/frc/PIDInterface.h
View File

@@ -1,36 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <wpi/deprecated.h>
namespace frc {
/**
* Interface for PID Control Loop.
*
* This class is provided by the OldCommands VendorDep
*
* @deprecated All APIs which use this have been deprecated.
*/
class PIDInterface {
public:
WPI_DEPRECATED("All APIs which use this have been deprecated.")
PIDInterface() = default;
PIDInterface(PIDInterface&&) = default;
PIDInterface& operator=(PIDInterface&&) = default;
virtual void SetPID(double p, double i, double d) = 0;
virtual double GetP() const = 0;
virtual double GetI() const = 0;
virtual double GetD() const = 0;
virtual void SetSetpoint(double setpoint) = 0;
virtual double GetSetpoint() const = 0;
virtual void Reset() = 0;
};
} // namespace frc

22
wpilibOldCommands/src/main/native/include/frc/PIDOutput.h
View File

@@ -1,22 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
namespace frc {
/**
* PIDOutput interface is a generic output for the PID class.
*
* MotorControllers use this class. Users implement this interface to allow for
* a PIDController to read directly from the inputs.
*
* This class is provided by the OldCommands VendorDep
*/
class PIDOutput {
public:
virtual void PIDWrite(double output) = 0;
};
} // namespace frc

38
wpilibOldCommands/src/main/native/include/frc/PIDSource.h
View File

@@ -1,38 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
namespace frc {
enum class PIDSourceType { kDisplacement, kRate };
/**
* PIDSource interface is a generic sensor source for the PID class.
*
* All sensors that can be used with the PID class will implement the PIDSource
* that returns a standard value that will be used in the PID code.
*
* This class is provided by the OldCommands VendorDep
*/
class PIDSource {
public:
virtual ~PIDSource() = default;
/**
* Set which parameter you are using as a process control variable.
*
* @param pidSource An enum to select the parameter.
*/
virtual void SetPIDSourceType(PIDSourceType pidSource);
virtual PIDSourceType GetPIDSourceType() const;
virtual double PIDGet() = 0;
protected:
PIDSourceType m_pidSource = PIDSourceType::kDisplacement;
};
} // namespace frc

72
wpilibOldCommands/src/main/native/include/frc/buttons/Button.h
View File

@@ -1,72 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "frc/buttons/Trigger.h"
#include "frc/commands/Command.h"
namespace frc {
/**
* This class provides an easy way to link commands to OI inputs.
*
* It is very easy to link a button to a command. For instance, you could link
* the trigger button of a joystick to a "score" command.
*
* This class represents a subclass of Trigger that is specifically aimed at
* buttons on an operator interface as a common use case of the more generalized
* Trigger objects. This is a simple wrapper around Trigger with the method
* names renamed to fit the Button object use.
*
* This class is provided by the OldCommands VendorDep
*/
class Button : public Trigger {
public:
Button() = default;
Button(Button&&) = default;
Button& operator=(Button&&) = default;
/**
* Specifies the command to run when a button is first pressed.
*
* @param command The pointer to the command to run
*/
virtual void WhenPressed(Command* command);
/**
* Specifies the command to be scheduled while the button is pressed.
*
* The command will be scheduled repeatedly while the button is pressed and
* will be canceled when the button is released.
*
* @param command The pointer to the command to run
*/
virtual void WhileHeld(Command* command);
/**
* Specifies the command to run when the button is released.
*
* The command will be scheduled a single time.
*
* @param command The pointer to the command to run
*/
virtual void WhenReleased(Command* command);
/**
* Cancels the specificed command when the button is pressed.
*
* @param command The command to be canceled
*/
virtual void CancelWhenPressed(Command* command);
/**
* Toggle the specified command when the button is pressed.
*
* @param command The command to be toggled
*/
virtual void ToggleWhenPressed(Command* command);
};
} // namespace frc

29
wpilibOldCommands/src/main/native/include/frc/buttons/ButtonScheduler.h
View File

@@ -1,29 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
namespace frc {
class Trigger;
class Command;
class ButtonScheduler {
public:
ButtonScheduler(bool last, Trigger* button, Command* orders);
virtual ~ButtonScheduler() = default;
ButtonScheduler(ButtonScheduler&&) = default;
ButtonScheduler& operator=(ButtonScheduler&&) = default;
virtual void Execute() = 0;
void Start();
protected:
bool m_pressedLast;
Trigger* m_button;
Command* m_command;
};
} // namespace frc

25
wpilibOldCommands/src/main/native/include/frc/buttons/CancelButtonScheduler.h
View File

@@ -1,25 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "frc/buttons/ButtonScheduler.h"
namespace frc {
class Trigger;
class Command;
class CancelButtonScheduler : public ButtonScheduler {
public:
CancelButtonScheduler(bool last, Trigger* button, Command* orders);
~CancelButtonScheduler() override = default;
CancelButtonScheduler(CancelButtonScheduler&&) = default;
CancelButtonScheduler& operator=(CancelButtonScheduler&&) = default;
void Execute() override;
};
} // namespace frc

25
wpilibOldCommands/src/main/native/include/frc/buttons/HeldButtonScheduler.h
View File

@@ -1,25 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "frc/buttons/ButtonScheduler.h"
namespace frc {
class Trigger;
class Command;
class HeldButtonScheduler : public ButtonScheduler {
public:
HeldButtonScheduler(bool last, Trigger* button, Command* orders);
~HeldButtonScheduler() override = default;
HeldButtonScheduler(HeldButtonScheduler&&) = default;
HeldButtonScheduler& operator=(HeldButtonScheduler&&) = default;
void Execute() override;
};
} // namespace frc

37
wpilibOldCommands/src/main/native/include/frc/buttons/InternalButton.h
View File

@@ -1,37 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "frc/buttons/Button.h"
namespace frc {
/**
* This Button is intended to be used within a program. The programmer can
* manually set its value. Also includes a setting for whether or not it should
* invert its value.
*
* This class is provided by the OldCommands VendorDep
*/
class InternalButton : public Button {
public:
InternalButton() = default;
explicit InternalButton(bool inverted);
~InternalButton() override = default;
InternalButton(InternalButton&&) = default;
InternalButton& operator=(InternalButton&&) = default;
void SetInverted(bool inverted);
void SetPressed(bool pressed);
bool Get() override;
private:
bool m_pressed = false;
bool m_inverted = false;
};
} // namespace frc

32
wpilibOldCommands/src/main/native/include/frc/buttons/JoystickButton.h
View File

@@ -1,32 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "frc/GenericHID.h"
#include "frc/buttons/Button.h"
namespace frc {
/**
* A Button} that gets its state from a GenericHID.
*
* This class is provided by the OldCommands VendorDep
*/
class JoystickButton : public Button {
public:
JoystickButton(GenericHID* joystick, int buttonNumber);
~JoystickButton() override = default;
JoystickButton(JoystickButton&&) = default;
JoystickButton& operator=(JoystickButton&&) = default;
bool Get() override;
private:
GenericHID* m_joystick;
int m_buttonNumber;
};
} // namespace frc

38
wpilibOldCommands/src/main/native/include/frc/buttons/NetworkButton.h
View File

@@ -1,38 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <memory>
#include <string_view>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableEntry.h>
#include "frc/buttons/Button.h"
namespace frc {
/**
* A that uses a NetworkTable boolean field.
*
* <p>This class is provided by the OldCommands VendorDep
*/
class NetworkButton : public Button {
public:
NetworkButton(std::string_view tableName, std::string_view field);
NetworkButton(std::shared_ptr<nt::NetworkTable> table,
std::string_view field);
~NetworkButton() override = default;
NetworkButton(NetworkButton&&) = default;
NetworkButton& operator=(NetworkButton&&) = default;
bool Get() override;
private:
nt::NetworkTableEntry m_entry;
};
} // namespace frc

39
wpilibOldCommands/src/main/native/include/frc/buttons/POVButton.h
View File

@@ -1,39 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "frc/GenericHID.h"
#include "frc/buttons/Button.h"
namespace frc {
/**
* A Button that gets its state from a POV on a GenericHID.
*
* <p>This class is provided by the OldCommands VendorDep
*/
class POVButton : public Button {
public:
/**
* Creates a POV button for triggering commands.
*
* @param joystick The GenericHID object that has the POV
* @param angle The desired angle in degrees (e.g. 90, 270)
* @param povNumber The POV number (@see GenericHID#GetPOV)
*/
POVButton(GenericHID& joystick, int angle, int povNumber = 0);
~POVButton() override = default;
POVButton(POVButton&&) = default;
POVButton& operator=(POVButton&&) = default;
bool Get() override;
private:
GenericHID* m_joystick;
int m_angle;
int m_povNumber;
};
} // namespace frc

25
wpilibOldCommands/src/main/native/include/frc/buttons/PressedButtonScheduler.h
View File

@@ -1,25 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "frc/buttons/ButtonScheduler.h"
namespace frc {
class Trigger;
class Command;
class PressedButtonScheduler : public ButtonScheduler {
public:
PressedButtonScheduler(bool last, Trigger* button, Command* orders);
~PressedButtonScheduler() override = default;
PressedButtonScheduler(PressedButtonScheduler&&) = default;
PressedButtonScheduler& operator=(PressedButtonScheduler&&) = default;
void Execute() override;
};
} // namespace frc

25
wpilibOldCommands/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h
View File

@@ -1,25 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "frc/buttons/ButtonScheduler.h"
namespace frc {
class Trigger;
class Command;
class ReleasedButtonScheduler : public ButtonScheduler {
public:
ReleasedButtonScheduler(bool last, Trigger* button, Command* orders);
~ReleasedButtonScheduler() override = default;
ReleasedButtonScheduler(ReleasedButtonScheduler&&) = default;
ReleasedButtonScheduler& operator=(ReleasedButtonScheduler&&) = default;
void Execute() override;
};
} // namespace frc

Some files were not shown because too many files have changed in this diff Show More