Squashed 'third_party/allwpilib_2019/' changes from 99e4f7dd2..c36bbcc9a
936627bd9 wpilibc: Remove direct CameraServer dependency (#1989)
8e333c0aa Use FPGA Time instead of wall clock time for odometry (#1996)
d4430b765 Gearsbot example: Use standard argument order (#1995)
75438ab2c Add RamseteCommand (#1951)
989df1b46 Bump Native Utils and OpenCV dependencies (#1993)
dbc33b61e Fix Timer usage in TrapezoidProfileCommand (#1992)
79f8c5644 Add TrapezoidProfileCommand (#1962)
9440edf2b Refactor TrajectoryGenerator (#1972)
73a30182c Add frc2::Timer (#1968)
36ea865ed Add toString for geometry and trajectory classes (#1991)
cbe05e7e8 Update ProfiledPIDController API (#1967)
d04eb3546 Deprecate old PID classes (#1964)
02264db69 Add JNI dependencies to myRobotCpp (#1980)
2a76c996e Use VID/PID detection for PS3Eye (#1977)
a3820bbdf Remove HAL_BaseInitialize (#1981)
a83fb4793 Update to 2020v5 image (#1983)
4b0ed910e Make SwerveDriveKinematics.toChassisSpeeds() public (#1976)
103c1b121 Remove DS caching from the HAL level (#1971)
6635ea75e Fix NPE in SendableRegistry.foreachLiveWindow() (#1974)
cfe23c5cd Fix grammar error in comment for configureButtonBindings (#1969)
4bde2654e Fix mac azure build (#1973)
4f034e6c1 generateTrajectory: default reversed param to false (#1953)
acf960f72 Sim GUI: Add option to disable outputs on DS disable
2d3dac99f Sim GUI: Handle low resolutions and scale default window positions
07c86e0cd Sim GUI: Support High DPI monitors
46ad95512 SimDeviceData: Add missing null check
5bce489b9 Add ProggyDotted font to imgui (both cmake and gradle)
55af553ac Simulation GUI: Map gamepad the same way as DS
c59f9cea5 CameraServer: Add VID/PID support for Linux USB devices (#1960)
3fc89c84d Make splinePointsFromSplines public (#1963)
2c5093797 Fix implicitly deleted move constructors (#1954)
f3ad927f4 Update Java SmartDashboard and LiveWindow to match C++
05c25deb7 Fix move handling of C++ Sendable in SmartDashboard and LiveWindow
d726591ce Fix Gazebo sim plugin build (#1959)
2ff694fa4 Unbreak gradle build when other compilers installed (#1958)
53816155b Improve command decorator names (#1945)
a38f183a9 Fix GenResources.cmake so it's usable in a submodule (#1956)
b3398dca3 Set gradlebase correctly for all examples (#1950)
2c311013d Add Aarch64Bionic platform detection (#1922)
c10f2003c Add generateTrajectory overload (#1944)
63cfa64fb Add getters for pose in odometry classes (#1943)
2402c2bad Fix C++ command group recursive constructor bug (#1941)
f4eedf597 Fix ConcurrentModificationException in CommandScheduler (#1938)
bb0b207d2 Fix array out of bounds exception caused by parallel race group (#1935)
7bd69e591 Fix typo in temperature (#1940)
ec9738245 Bump to 2020 v4 image (#1931)
46303a822 Add messaging to extension loading in the HAL (#1926)
d169d6be9 Set extract_static for Doxygen config so that static members show up (#1930)
4e183eb10 Bump to 2020 v3 image (#1929)
84c185803 LiveWindow: catch errors in callback/builder functions (#1921)
0e3b0f3da Remove deprecated free() calls (#1925)
7f839b87c Remove timeouts from azure builds (#1924)
45b766a5d Fix main thread ID being potentially incorrect for simulation (#1923)
56d782b16 Add secondary camera name for PS3Eye (#1920)
2b4894038 Add simulation GUI plugin
f97d16073 Add imgui build to cmake
55a844a3e HAL sim: Add encoder channel B access
10deba854 Remove sendables from SendableRegistry when close() is called (#1917)
a9f0e4668 Implement sim devices for ADXL345, ADXL362, ADXRS450, Ultrasonic
aa9064586 Add ability to associate other devices with a SimDevice
81c2c8a7d Add simulation generic device/value support
e8d6f8a2c Move mockdata/HAL_Value.h to hal/Value.h
1b266717a Add simulation module support to cmake build (#1906)
fb8f3bd06 Add testbench yaml file (#1915)
846d8def0 Update to 2020 v2 image (#1913)
d6ac6e512 Fix PortForwarder package declaration (#1912)
227157086 Fix PS3Eye exposure setting (#1911)
885744d7e Add myRobot C++ version to cmake build (#1907)
366091fa8 Document that ConditionalCommand requires all subsystems (#1909)
c58b072c8 Fix Drive usage reporting order (#1908)
762c88adb Update compiler versions in readme (#1905)
af8ce568d Add Ramsete unicycle controller (#1790)
b2c2934d0 Fix javadoc warnings about invalid usage of ">" (#1904)
cce26ec78 Replace CRLF line endings with LF (#1902)
cb54602d4 Add support for writing RTR CAN Frames to the CAN API (#1900)
9f740e590 Use OS for serial port instead of NI VISA (#1875)
b23baf611 Add ability to run robot main loop in a separate thread (#1895)
457f94ba2 Add trajectory generation using hermite splines (#1843)
fd612052f Update native utils to use new frchome directory (#1884)
8858ec55c Remove periodic can read call (#1868)
41efb8015 Update CANAPITypes.h (#1860)
c93be1b2d Remove LabVIEW HAL support (#1901)
680f8919e Remove eigen, units and libuv from doxygen generation (#1898)
c5812524f Bump GradleJNI plugin version (#1899)
971303da8 Add PortForwarder class (#1890)
50db77bf2 Fix wpiutil cmake eigen install source directory (#1891)
85d42c199 C++ PIDCommand: Add GetMeasurement() and UseOutput() (#1892)
2dfbb855d wpilibj: Fix SwerveDriveKinematics ctor parameter name (#1889)
471f375a3 Simplify Sendable interface (#1864)
1d8c4d016 Replace ::value and ::type with _v and _t suffixes (#1885)
a5650b943 Add Units Utility class for Java (#1829)
904479ad4 Deprecate GearTooth class for removal (#1878)
86b666bba Add equality comparator to geometry classes (#1882)
62f07c182 Make one-arg Rotation2d constructor implicit (#1883)
f405582f8 Add kinematics suite (#1787)
561cbbd14 Deprecate Filter class for removal (#1876)
84e2973aa Remove unused include from Filesystem.h (#1877)
f49859ebf Remove NI VISA headers, as they are now included in NI Libraries (#1879)
bc59db5e6 Rename DEBUG macro to DEBUG0 (#1871)
dd928b4cb Remove JNI logging (#1872)
3e0f7d099 Use units for new NotifierCommand (#1869)
5ffe15d5f Remove ability to build all combined artifacts (#1867)
516cbef2c Remove RoboRIO ifdef from simulation headers (#1859)
9b6ffc201 Replace SetOutputRange() with SetIntegratorRange()
ff8b8f0a8 Remove percent tolerance from PID controller
0ca8d667d Clean up AutoCloseable and other Java warnings (#1866)
7112add67 Watchdog: use units::second_t instead of double (#1863)
761bc3ef8 Change C++ WaitCommand to use units (#1865)
1fb301123 Add MathUtils.clamp() for Java (#1861)
eb3e0c9c9 Fix cmake Eigen include directory (#1862)
2250b7fbe Rename GearsBotNew example to GearsBot
c9f9feff1 Replace deprecated API usage in C++ examples
d6b9c7e14 CONTRIBUTING.md: Point to frc-docs instead of screensteps (#1858)
d10a1a797 Fix eigen build in vcpkg (#1856)
2bdb44325 Add frc2 includes to list of "other lib" regexes (#1855)
4b2b21d24 Replace outdated Java collections (#508)
8993ce5bf Move Eigen headers out of main include folder (#1854)
0f532a117 Add PWMSparkMax (#1751)
f7ad363d8 Add jni cross compile options for aarch64 (#1853)
9afea3340 Add support for aarch64 jetson bionic builds (#1844)
d787b5d60 Add more items to .gitignore (#1850)
5dd0d1b7d Use units in SPI
07ac711b3 Fix units deprecated warning in IterativeRobot
decfd858b Correctly report -1 for POV on disconnected joystick (#1852)
076ed7770 Add new C++ Command framework (#1785)
a0be07c37 Refactor HAL handle move construction/assignment (#1845)
558c38308 Add new Java Command framework (#1682)
1379735af Delete RobotState and SensorUtil constructors (#1847)
e3d86fee4 Move circular buffer class from wpilib to wpiutil (#1840)
4cd8a5667 TimedRobot.cpp: Fix deprecation warning (#1846)
b2861f894 Use 2020 artifacts and artifactory server (#1838)
98cc32703 Update to use artifactory to publish artifacts (#1833)
fa0640300 Move drive integration tests into wpilibj/src/test (#1836)
e716c36b8 Fix Nat.java generation to be incremental (#1831)
9fd2b5e3f Fix MSVC builds on cmake windows in vcpkg (#1835)
7e95010a2 Add compile-time EJML matrix wrapper to wpiutil (#1804)
3ebc5a6d3 Add ProfiledPIDController
fc98a79db Clean up PIDController interface in preparation for ProfiledPIDController
fdc098267 Fix compilation error in elevator trapezoid profile example (#1826)
a3dd84e85 Make XBoxController Button enum public (#1823)
a216b9e9e Add TrapezoidProfile example (#1814)
8f386f6bb wpilibc: Add unit-safety to C++ geometry classes (#1811)
c07ac2353 wpilibc: Add overloads for units (#1815)
f1d71da8a Move GetStackTrace and Demangle to wpiutil, add Windows support (#1819)
ef037457e Make LinearFilter copyable and moveable (#1789)
76930250c Remove objective-cpp support (#1816)
1c246418f Move TrapezoidProfileTest to trajectory folder (#1812)
95a54a0f2 Add java arcade drive example (#1810)
a4530243e HAL sim: Fix incorrectly setting dio port to initialized on cleanup (#1813)
09d00a622 Update Java examples to use new PIDController (#1809)
ba9b51742 Add missing Java examples (#841)
6411bd79c InterruptableSensorBase: Fix callback function deletion (#1807)
810e58ea8 I2C: Add tip about writeBulk() to transaction() (#1806)
607d6c148 Fix wpilibj integration tests jar name (#1808)
c9873e81b Remove PIDControllerRunner and mutex from new PIDController (#1795)
98d0706de Fix cscore build with OpenCV 4 (#1803)
fbe67c90c Make Sendable setters synchronous (#1799)
c67a488a0 Format SendableBuilderImpl javadocs (#1802)
8e93ce892 Fix PIDControllerRunner member destruction order (#1801)
c98ca7310 Add EJML dependency to wpiutil (#1769)
3b12276bc SendableBase: remove unnecessary synchronization (#1797)
e6d348f38 Fix missing default name in Java PIDController (#1792)
df12fc2a8 Java cleanups (#1776)
39561751f Update GradleVSCode version (#1786)
37d316aa0 Add C++20 std::math constants shim (#1788)
dd4310959 Deprecate frc/WPILib.h (#1779)
823174f30 Update native utils to 2020.0.4 (#1783)
37c695266 Squelch -Wdeprecated-copy for Eigen with GCC >= 9
04c9b000f Revert "Fix build of Eigen 3.3.7 with GCC 9"
ca3e71e21 wpiutil: Fix Process::Spawn() (#1778)
d946d5a2b Fix Eigen compilation errors and add tests (#1777)
8b1b9ac75 Fix build of Eigen 3.3.7 with GCC 9
2f680ba99 Add Eigen linear algebra library
a885db7d4 Make MotorEncoderTest use LinearFilter (#1775)
ee2410169 Add geometry classes (#1766)
48fe54271 Add HALSIM_SetSendError implementation (#1773)
dff58c87f Fix unused warning in release build (#1771)
dde61aad3 Remove TimerEventHandler typedef from Notifier class (#1767)
0f6ef80ab Add RobotState#IsEStopped and DriverStation#IsEStopped (#952)
e48886187 Move unit tests from integration test suite (#1170)
dffa1a5cb Make null checks more descriptive (#1688)
fe59d854d Notifier: add null check (#1684)
10731f3d6 Update uv Udp wrapper for latest features
89f7b72b6 Update libuv to 1.30.1 release
85f2f8740 wpiutil: Add unique_function (#1761)
73ec94078 Remove SampleRobot (#1658)
62be0392b Replace std::lock_guard and std::lock with std::scoped_lock (#1758)
24d31df55 Make sure move constructor is generated for TrapezoidProfile (#1757)
841ef5d73 Remove template types from lock RAII wrapper usages (#1756)
e582518ba Fix some move constructors (#1754)
8757bc471 Remove pre-C++17 shims (#1752)
ea9512977 Add replacement PIDController class (#1300)
9b798d228 Add TrapezoidProfile class (#1673)
804926fb5 Unconditionally skip athena builds for sim (#1748)
118e9d29d Add C++14 units library (#1749)
c705953d7 Add usage reporting to LinearFilter (#1750)
852d1b9ca Don't cross-build gazebo for raspbian (#1747)
eedb3a1ad Fix GCC 9 warnings (#1730)
60dce66a4 Remove wpi::ArrayRef std::initializer_list constructor (#1745)
9e19b29c3 Use base azure image for primary wpilib build (#1744)
299425071 Update jni library, fix cross builds of the jni symbol check (#1742)
a6b0e9b85 Only disable execution of cross compile google tests (#1741)
3c2093119 Use docker container to run wpiformat (#1740)
5fe2eebce Revert "Don't build halsim_gazebo on raspbian (#1737)" (#1743)
4b1b92bb7 Replace wpi::optional with C++17 std::optional (#1732)
0fbb0d989 Update to 2020 compilers (#1733)
2dc94e605 Disable google tests on cross compilers (#1738)
d9cb57a42 Don't build halsim_gazebo on raspbian (#1737)
f7cfdd7ce Replace crlf line endings with lf (#1731)
b6d5d90d9 Add JaCoCo Support (#1734)
c7ab2baa6 Add way to disable the jni check tasks from a property (#1736)
0c45c5b7e Fix skip athena and skip raspbian flags (#1735)
3dfb01d45 Update to new Native Utils (#1696)
30e936837 Clean up LinearDigitalFilter class (#782)
311e2de4c Remove deprecated Joystick constants (#1715)
c08fd6682 Update CAN manufacturer list (#1706)
258bba0c2 ErrorBase and WPIError improvements (#1727)
372ca4f45 cmake: Enable googletest unit tests (#1720)
223d47af0 HALSIM: support mocking of HAL_SendError() (#1728)
55cb683db Change compiler flags to C++17 (#1723)
ee8a33c56 wpiutil: SafeThread: Add thread id, support getting shared_ptr (#1722)
61426d08d wpiutil: Signal: make operator() const (#1721)
b630b63ef Remove functions in LiveWindow deprecated since 2018 (#1716)
1d0c05d4f Styleguide fixes for #1718 (#1719)
f07569df1 Fix newer GCC/clang compiler warnings (#1718)
0120f3124 C++ SPI: Fix SetClockRate to take int (#1717)
c2829ed98 Configure gradle to ignore unresolved headers (#1711)
221e66f46 Allow disabling static init of JNI libraries (#1672)
738852e11 cmake: Add cross toolchain files for Rio and Pi (#1710)
27b697b08 Remove frc directory include shims (#1714)
9e45373a7 Remove functions and classes deprecated for 2018 season (#1059)
eeb1025ac SPI: Report port as instance for usage reporting (#1704)
bc6f1e246 Windows compiler options improvements (#1709)
bb48ae391 cmake: Move example programs into folders (#1654)
221011494 Update for C++17 and fix MSVC warnings (#1694)
fb1239a2a Add raw sources and sinks to cscore (#1670)
7de947734 Add lambda overloads for interrupts (#1636)
90957aeea Move libuv to its own subfolder in build (#1661)
47aae502a Styleguide fixes (#1702)
0bff98b5e Correct DifferentialDrive::ArcadeDrive param docs (#1698)
b52e40b80 Allow widgets to be added by passing value suppliers (#1690)
4a00cd77b Add usage reporting for the Shuffleboard API (#1685)
e25e515f2 Publish artifacts on azure (#1678)
322ef9b96 Force Java 11, fix javadoc generation (#1695)
d42ef5df0 Fix Watchdog print formatting (#1693)
f432f65be Update copyright year in license to 2019 (#1524)
1726b77ac wpiutil: uv: Remove copy from SimpleBufferPool (#1680)
620bec9ca wpiutil: uv: Add LoopClosing status to Handle (#1647)
7cd6e2e7f UsbCamera: Solve race in windows initialization (#1638)
7732836bd Completely disable watchdog tests on mac (#1679)
698edfda9 Remove framework load, disable mac timeout test (#1676)
1c454b000 Add Shuffleboard calls to IterativeRobotBase in C++ (#1607)
f42905b32 Include missing headers in HAL.h (#1660)
bdc822fad Only generate passthrough URLs for RoboRIO (#1624)
d3affb16b Make failure of HAL_GetFPGATime() more descriptive (#1633)
2de3bf7f5 Update LLVM from stable upstream (#1653)
3cf4f38f5 Fix build on macos10.14.4 (#1648)
4e0c10f48 Fix CAN Clean using wrong ID (#1668)
3b0631324 Fix Gray to BGR conversion in CameraServer (#1665)
6cd1c73ef Fix GUID comparison creating weird symbol (#1659)
063bbab6f MavenArtifacts.md: update links to HTTPS (#1674)
aab4c494d Fix type in build.gradle (#1604)
bf46af260 Disable extraneous data warnings in libjpeg (#1630)
655763a9a Limit length of message sent to DS SendError call (#1618)
a095ec2d8 Fix linker errors with free functions in Threads.h (#1625)
12ab035aa Fix receive side of LabVIEW USB streams (#1621)
Change-Id: Ibd382e1a48925c200850cf90a8121e35c0fcffe3
git-subtree-dir: third_party/allwpilib_2019
git-subtree-split: c36bbcc9a9095489fc078229db4fba3ecd0f9b78
diff --git a/wpilibjExamples/build.gradle b/wpilibjExamples/build.gradle
index 55087ba..7b65086 100644
--- a/wpilibjExamples/build.gradle
+++ b/wpilibjExamples/build.gradle
@@ -1,5 +1,4 @@
apply plugin: 'java'
-apply plugin: 'pmd'
ext {
useJava = true
@@ -21,16 +20,20 @@
compile project(':cameraserver')
}
-pmd {
- consoleOutput = true
- reportsDir = file("$project.buildDir/reports/pmd")
- ruleSetFiles = files(new File(rootDir, "styleguide/pmd-ruleset.xml"))
- ruleSets = []
+if (!project.hasProperty('skipPMD')) {
+ apply plugin: 'pmd'
+
+ pmd {
+ consoleOutput = true
+ reportsDir = file("$project.buildDir/reports/pmd")
+ ruleSetFiles = files(new File(rootDir, "styleguide/pmd-ruleset.xml"))
+ ruleSets = []
+ }
}
gradle.projectsEvaluated {
tasks.withType(JavaCompile) {
- options.compilerArgs << "-Xlint:unchecked" << "-Xlint:deprecation"
+ options.compilerArgs << "-Xlint:unchecked" << "-Xlint:deprecation" << "-Werror"
}
}
diff --git a/wpilibjExamples/publish.gradle b/wpilibjExamples/publish.gradle
index b2aa3aa..a2c0746 100644
--- a/wpilibjExamples/publish.gradle
+++ b/wpilibjExamples/publish.gradle
@@ -1,12 +1,5 @@
apply plugin: 'maven-publish'
-def pubVersion
-if (project.hasProperty("publishVersion")) {
- pubVersion = project.publishVersion
-} else {
- pubVersion = WPILibVersion.version
-}
-
def baseExamplesArtifactId = 'examples'
def baseTemplatesArtifactId = 'templates'
def baseCommandsArtifactId = 'commands'
@@ -72,7 +65,7 @@
artifactId = baseExamplesArtifactId
groupId artifactGroupId
- version pubVersion
+ version wpilibVersioning.version.get()
}
templates(MavenPublication) {
@@ -80,7 +73,7 @@
artifactId = baseTemplatesArtifactId
groupId artifactGroupId
- version pubVersion
+ version wpilibVersioning.version.get()
}
commands(MavenPublication) {
@@ -88,7 +81,7 @@
artifactId = baseCommandsArtifactId
groupId artifactGroupId
- version pubVersion
+ version wpilibVersioning.version.get()
}
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/emptyclass/ReplaceMeEmptyClass.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/emptyclass/ReplaceMeEmptyClass.java
index 9498348..e2d866e 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/emptyclass/ReplaceMeEmptyClass.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/emptyclass/ReplaceMeEmptyClass.java
@@ -1,14 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.commands.emptyclass;
-
-/**
- * Add your docs here.
- */
-public class ReplaceMeEmptyClass {
-}
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.commands.emptyclass;
+
+/**
+ * Add your docs here.
+ */
+public class ReplaceMeEmptyClass {
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Main.java
similarity index 89%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Main.java
index 787aff0..438697e 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.arcadedrive;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java
new file mode 100644
index 0000000..f002618
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.arcadedrive;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class.
+ * Runs the motors with arcade steering.
+ */
+public class Robot extends TimedRobot {
+ private final PWMVictorSPX m_leftMotor = new PWMVictorSPX(0);
+ private final PWMVictorSPX m_rightMotor = new PWMVictorSPX(1);
+ private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final Joystick m_stick = new Joystick(0);
+
+ @Override
+ public void teleopPeriodic() {
+ // Drive with arcade drive.
+ // That means that the Y axis drives forward
+ // and backward, and the X turns left and right.
+ m_robotDrive.arcadeDrive(m_stick.getY(), m_stick.getX());
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Main.java
similarity index 89%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Main.java
index 787aff0..ff5c19c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.canpdp;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Robot.java
new file mode 100644
index 0000000..7a0831b
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Robot.java
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.canpdp;
+
+import edu.wpi.first.wpilibj.PowerDistributionPanel;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * This is a sample program showing how to retrieve information from the Power Distribution Panel
+ * via CAN. The information will be displayed under variables through the SmartDashboard.
+ */
+public class Robot extends TimedRobot {
+ private static final int kPDPId = 0;
+
+ private final PowerDistributionPanel m_pdp = new PowerDistributionPanel(kPDPId);
+
+ @Override
+ public void robotPeriodic() {
+ /*
+ * Get the current going through channel 7, in Amperes. The PDP returns the
+ * current in increments of 0.125A. At low currents
+ * the current readings tend to be less accurate.
+ */
+ SmartDashboard.putNumber("Current Channel 7", m_pdp.getCurrent(7));
+
+ /*
+ * Get the voltage going into the PDP, in Volts.
+ * The PDP returns the voltage in increments of 0.05 Volts.
+ */
+ SmartDashboard.putNumber("Voltage", m_pdp.getVoltage());
+
+ /*
+ * Retrieves the temperature of the PDP, in degrees Celsius.
+ */
+ SmartDashboard.putNumber("Temperature", m_pdp.getTemperature());
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
new file mode 100644
index 0000000..9f33156
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.differentialdrivebot;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+
+/**
+ * Represents a differential drive style drivetrain.
+ */
+public class Drivetrain {
+ public static final double kMaxSpeed = 3.0; // meters per second
+ public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
+
+ private static final double kTrackWidth = 0.381 * 2; // meters
+ private static final double kWheelRadius = 0.0508; // meters
+ private static final int kEncoderResolution = 4096;
+
+ private final Spark m_leftMaster = new Spark(1);
+ private final Spark m_leftFollower = new Spark(2);
+ private final Spark m_rightMaster = new Spark(3);
+ private final Spark m_rightFollower = new Spark(4);
+
+ private final Encoder m_leftEncoder = new Encoder(0, 1);
+ private final Encoder m_rightEncoder = new Encoder(0, 1);
+
+ private final SpeedControllerGroup m_leftGroup
+ = new SpeedControllerGroup(m_leftMaster, m_leftFollower);
+ private final SpeedControllerGroup m_rightGroup
+ = new SpeedControllerGroup(m_rightMaster, m_rightFollower);
+
+ private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+ private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
+ private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
+
+ private final DifferentialDriveKinematics m_kinematics
+ = new DifferentialDriveKinematics(kTrackWidth);
+
+ private final DifferentialDriveOdometry m_odometry
+ = new DifferentialDriveOdometry(m_kinematics);
+
+ /**
+ * Constructs a differential drive object.
+ * Sets the encoder distance per pulse and resets the gyro.
+ */
+ public Drivetrain() {
+ m_gyro.reset();
+
+ // Set the distance per pulse for the drive encoders. We can simply use the
+ // distance traveled for one rotation of the wheel divided by the encoder
+ // resolution.
+ m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+ m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+ }
+
+ /**
+ * Returns the angle of the robot as a Rotation2d.
+ *
+ * @return The angle of the robot.
+ */
+ public Rotation2d getAngle() {
+ // Negating the angle because WPILib gyros are CW positive.
+ return Rotation2d.fromDegrees(-m_gyro.getAngle());
+ }
+
+ /**
+ * Returns the current wheel speeds.
+ *
+ * @return The current wheel speeds.
+ */
+ public DifferentialDriveWheelSpeeds getCurrentSpeeds() {
+ return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
+ }
+
+ /**
+ * Sets the desired wheel speeds.
+ *
+ * @param speeds The desired wheel speeds.
+ */
+ public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
+ double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(),
+ speeds.leftMetersPerSecond);
+ double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(),
+ speeds.rightMetersPerSecond);
+ m_leftGroup.set(leftOutput);
+ m_rightGroup.set(rightOutput);
+ }
+
+ /**
+ * Drives the robot with the given linear velocity and angular velocity.
+ *
+ * @param xSpeed Linear velocity in m/s.
+ * @param rot Angular velocity in rad/s.
+ */
+ @SuppressWarnings("ParameterName")
+ public void drive(double xSpeed, double rot) {
+ var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot));
+ setSpeeds(wheelSpeeds);
+ }
+
+ /**
+ * Updates the field-relative position.
+ */
+ public void updateOdometry() {
+ m_odometry.update(getAngle(), getCurrentSpeeds());
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Main.java
similarity index 88%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Main.java
index 787aff0..1f2b319 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.differentialdrivebot;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java
new file mode 100644
index 0000000..10df8b6
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.differentialdrivebot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+import static edu.wpi.first.wpilibj.examples.differentialdrivebot.Drivetrain.kMaxAngularSpeed;
+import static edu.wpi.first.wpilibj.examples.differentialdrivebot.Drivetrain.kMaxSpeed;
+
+public class Robot extends TimedRobot {
+ private final XboxController m_controller = new XboxController(0);
+ private final Drivetrain m_drive = new Drivetrain();
+
+ @Override
+ public void autonomousPeriodic() {
+ teleopPeriodic();
+ m_drive.updateOdometry();
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ // Get the x speed. We are inverting this because Xbox controllers return
+ // negative values when we push forward.
+ final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * kMaxSpeed;
+
+ // Get the rate of angular rotation. We are inverting this because we want a
+ // positive value when we pull to the left (remember, CCW is positive in
+ // mathematics). Xbox controllers return positive values when you pull to
+ // the right by default.
+ final var rot = -m_controller.getX(GenericHID.Hand.kRight) * kMaxAngularSpeed;
+
+ m_drive.drive(xSpeed, rot);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Main.java
similarity index 88%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Main.java
index 787aff0..5df92a6 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.elevatorprofiledpid;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
new file mode 100644
index 0000000..a48909f
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.elevatorprofiledpid;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+public class Robot extends TimedRobot {
+ private static double kDt = 0.02;
+
+ private final Joystick m_joystick = new Joystick(1);
+ private final Encoder m_encoder = new Encoder(1, 2);
+ private final Spark m_motor = new Spark(1);
+
+ // Create a PID controller whose setpoint's change is subject to maximum
+ // velocity and acceleration constraints.
+ private final TrapezoidProfile.Constraints m_constraints =
+ new TrapezoidProfile.Constraints(1.75, 0.75);
+ private final ProfiledPIDController m_controller =
+ new ProfiledPIDController(1.3, 0.0, 0.7, m_constraints, kDt);
+
+ @Override
+ public void robotInit() {
+ m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ if (m_joystick.getRawButtonPressed(2)) {
+ m_controller.setGoal(5);
+ } else if (m_joystick.getRawButtonPressed(3)) {
+ m_controller.setGoal(0);
+ }
+
+ // Run controller and update motor output
+ m_motor.set(m_controller.calculate(m_encoder.getDistance()));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Main.java
similarity index 88%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Main.java
index 787aff0..12255cd 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
new file mode 100644
index 0000000..fddd417
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+public class Robot extends TimedRobot {
+ private static double kDt = 0.02;
+
+ private final Joystick m_joystick = new Joystick(1);
+ private final Encoder m_encoder = new Encoder(1, 2);
+ private final Spark m_motor = new Spark(1);
+ private final PIDController m_controller = new PIDController(1.3, 0.0, 0.7, kDt);
+
+ private final TrapezoidProfile.Constraints m_constraints =
+ new TrapezoidProfile.Constraints(1.75, 0.75);
+ private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
+ private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
+
+ @Override
+ public void robotInit() {
+ m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ if (m_joystick.getRawButtonPressed(2)) {
+ m_goal = new TrapezoidProfile.State(5, 0);
+ } else if (m_joystick.getRawButtonPressed(3)) {
+ m_goal = new TrapezoidProfile.State(0, 0);
+ }
+
+ // Create a motion profile with the given maximum velocity and maximum
+ // acceleration constraints for the next setpoint, the desired goal, and the
+ // current setpoint.
+ var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
+
+ // Retrieve the profiled setpoint for the next timestep. This setpoint moves
+ // toward the goal while obeying the constraints.
+ m_setpoint = profile.calculate(kDt);
+
+ double output = m_controller.calculate(m_encoder.getDistance(),
+ m_setpoint.position);
+
+ // Run controller with profiled setpoint and update motor output
+ m_motor.set(output);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Main.java
similarity index 89%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Main.java
index 787aff0..da0be79 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.encoder;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java
new file mode 100644
index 0000000..4bb26ea
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.encoder;
+
+import edu.wpi.first.wpilibj.CounterBase;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * Sample program displaying the value of a quadrature encoder on the SmartDashboard. Quadrature
+ * Encoders are digital sensors which can detect the amount the encoder has rotated since starting
+ * as well as the direction in which the encoder shaft is rotating. However, encoders can not tell
+ * you the absolute position of the encoder shaft (ie, it considers where it starts to be the zero
+ * position, no matter where it starts), and so can only tell you how much the encoder has rotated
+ * since starting. Depending on the precision of an encoder, it will have fewer or greater ticks per
+ * revolution; the number of ticks per revolution will affect the conversion between ticks and
+ * distance, as specified by DistancePerPulse. One of the most common uses of encoders is in the
+ * drivetrain, so that the distance that the robot drives can be precisely controlled during the
+ * autonomous mode.
+ */
+public class Robot extends TimedRobot {
+ /**
+ * The Encoder object is constructed with 4 parameters, the last two being optional. The first two
+ * parameters (1, 2 in this case) refer to the ports on the roboRIO which the encoder uses.
+ * Because a quadrature encoder has two signal wires, the signal from two DIO ports on the roboRIO
+ * are used. The third (optional) parameter is a boolean which defaults to false. If you set this
+ * parameter to true, the direction of the encoder will be reversed, in case it makes more sense
+ * mechanically. The final (optional) parameter specifies encoding rate (k1X, k2X, or k4X) and
+ * defaults to k4X. Faster (k4X) encoding gives greater positional precision but more noise in the
+ * rate.
+ */
+ private final Encoder m_encoder =
+ new Encoder(1, 2, false, CounterBase.EncodingType.k4X);
+
+ @Override
+ public void robotInit() {
+ /*
+ * Defines the number of samples to average when determining the rate.
+ * On a quadrature encoder, values range from 1-255;
+ * larger values result in smoother but potentially
+ * less accurate rates than lower values.
+ */
+ m_encoder.setSamplesToAverage(5);
+
+ /*
+ * Defines how far the mechanism attached to the encoder moves per pulse. In
+ * this case, we assume that a 360 count encoder is directly
+ * attached to a 3 inch diameter (1.5inch radius) wheel,
+ * and that we want to measure distance in inches.
+ */
+ m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);
+
+ /*
+ * Defines the lowest rate at which the encoder will
+ * not be considered stopped, for the purposes of
+ * the GetStopped() method. Units are in distance / second,
+ * where distance refers to the units of distance
+ * that you are using, in this case inches.
+ */
+ m_encoder.setMinRate(1.0);
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ SmartDashboard.putNumber("Encoder Distance", m_encoder.getDistance());
+ SmartDashboard.putNumber("Encoder Rate", m_encoder.getRate());
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
index 39a92e5..39840f7 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
@@ -23,6 +23,16 @@
"mainclass": "Main"
},
{
+ "name": "Arcade Drive",
+ "description": "Demonstrates the use of the DifferentialDrive class to drive a robot with Arcade Drive.",
+ "tags": [
+ "Getting Started with Java"
+ ],
+ "foldername": "arcadedrive",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
"name": "Mecanum Drive",
"description": "Demonstrate the use of the RobotDrive class doing teleop driving with Mecanum steering",
"tags": [
@@ -36,6 +46,55 @@
"mainclass": "Main"
},
{
+ "name": "PDP CAN Monitoring",
+ "description": "Demonstrate using CAN to monitor the voltage, current, and temperature in the Power Distribution Panel.",
+ "tags": [
+ "Complete List",
+ "CAN",
+ "Sensors"
+ ],
+ "foldername": "canpdp",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
+ "name": "Solenoids",
+ "description": "Demonstrate controlling a single and double solenoid from Joystick buttons.",
+ "tags": [
+ "Actuators",
+ "Joystick",
+ "Pneumatics",
+ "Complete List"
+ ],
+ "foldername": "solenoid",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
+ "name": "Encoder",
+ "description": "Demonstrate displaying the value of a quadrature encoder on the SmartDashboard.",
+ "tags": [
+ "Complete List",
+ "Digital",
+ "Sensors"
+ ],
+ "foldername": "encoder",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
+ "name": "Relay",
+ "description": "Demonstrate controlling a Relay from Joystick buttons.",
+ "tags": [
+ "Actuators",
+ "Joystick",
+ "Complete List"
+ ],
+ "foldername": "relay",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
"name": "Ultrasonic",
"description": "Demonstrate maintaining a set distance using an ultrasonic sensor.",
"tags": [
@@ -73,6 +132,32 @@
"mainclass": "Main"
},
{
+ "name": "Elevator with trapezoid profiled PID",
+ "description": "An example to demonstrate the use of an encoder and trapezoid profiled PID control to reach elevator position setpoints.",
+ "tags": [
+ "Digital",
+ "Sensors",
+ "Actuators",
+ "Joystick"
+ ],
+ "foldername": "elevatortrapezoidprofile",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
+ "name": "Elevator with profiled PID controller",
+ "description": "An example to demonstrate the use of an encoder and trapezoid profiled PID control to reach elevator position setpoints.",
+ "tags": [
+ "Digital",
+ "Sensors",
+ "Actuators",
+ "Joystick"
+ ],
+ "foldername": "elevatorprofiledpid",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
"name": "Gyro",
"description": "An example program showing how to drive straight with using a gyro sensor.",
"tags": [
@@ -137,7 +222,7 @@
},
{
"name": "GearsBot",
- "description": "A fully functional example CommandBased program for WPIs GearsBot robot. This code can run on your computer if it supports simulation.",
+ "description": "A fully functional example CommandBased program for WPIs GearsBot robot, ported to the new CommandBased library. This code can run on your computer if it supports simulation.",
"tags": [
"Complete Robot"
],
@@ -197,5 +282,116 @@
"foldername": "shuffleboard",
"gradlebase": "java",
"mainclass": "Main"
+ },
+ {
+ "name": "'Traditional' Hatchbot",
+ "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API. Written in the 'traditional' style, i.e. commands are given their own classes.",
+ "tags": [
+ "Complete robot",
+ "Command-based"
+ ],
+ "foldername": "hatchbottraditional",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
+ "name": "'Inlined' Hatchbot",
+ "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API. Written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
+ "tags": [
+ "Complete robot",
+ "Command-based",
+ "Lambdas"
+ ],
+ "foldername": "hatchbotinlined",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
+ "name": "Select Command Example",
+ "description": "An example showing how to use the SelectCommand class from the experimental command framework rewrite.",
+ "tags": [
+ "Command-based"
+ ],
+ "foldername": "selectcommand",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
+ "name": "Scheduler Event Logging",
+ "description": "An example showing how to use Shuffleboard to log Command events from the CommandScheduler in the experimental command framework rewrite",
+ "tags": [
+ "Command-based",
+ "Shuffleboard"
+ ],
+ "foldername": "schedulereventlogging",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
+ "name": "Frisbeebot",
+ "description": "An example robot project for a simple frisbee shooter for the 2013 FRC game, Ultimate Ascent, demonstrating use of PID functionality in the command framework",
+ "tags": [
+ "Command-based",
+ "PID"
+ ],
+ "foldername": "frisbeebot",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
+ "name": "Gyro Drive Commands",
+ "description": "An example command-based robot project demonstrating simple PID functionality utilizing a gyroscope to keep a robot driving straight and to turn to specified angles.",
+ "tags": [
+ "Command-based",
+ "PID",
+ "Gyro"
+ ],
+ "foldername": "gyrodrivecommands",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
+ "name": "SwerveBot",
+ "description": "An example program for a swerve drive that uses swerve drive kinematics and odometry.",
+ "tags": [
+ "SwerveBot"
+ ],
+ "foldername": "swervebot",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
+ "name": "MecanumBot",
+ "description": "An example program for a mecanum drive that uses mecanum drive kinematics and odometry.",
+ "tags": [
+ "MecanumBot"
+ ],
+ "foldername": "mecanumbot",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
+ "name": "DifferentialDriveBot",
+ "description": "An example program for a differential drive that uses differential drive kinematics and odometry.",
+ "tags": [
+ "MecanumBot"
+ ],
+ "foldername": "differentialdrivebot",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
+ "name:": "RamseteCommand",
+ "description": "An example command-based robot demonstrating the use of a RamseteCommand to follow a pregenerated trajectory.",
+ "tags": [
+ "RamseteCommand",
+ "PID",
+ "Ramsete",
+ "Trajectory",
+ "Path following"
+ ],
+ "foldername": "ramsetecommand",
+ "gradlebase": "java",
+ "mainclass": "Main"
}
]
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
new file mode 100644
index 0000000..384fb98
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.frisbeebot;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kLeftMotor1Port = 0;
+ public static final int kLeftMotor2Port = 1;
+ public static final int kRightMotor1Port = 2;
+ public static final int kRightMotor2Port = 3;
+
+ public static final int[] kLeftEncoderPorts = new int[]{0, 1};
+ public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final boolean kLeftEncoderReversed = false;
+ public static final boolean kRightEncoderReversed = true;
+
+ public static final int kEncoderCPR = 1024;
+ public static final double kWheelDiameterInches = 6;
+ public static final double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
+ }
+
+ public static final class ShooterConstants {
+ public static final int[] kEncoderPorts = new int[]{4, 5};
+ public static final boolean kEncoderReversed = false;
+ public static final int kEncoderCPR = 1024;
+ public static final double kEncoderDistancePerPulse =
+ // Distance units will be rotations
+ 1. / (double) kEncoderCPR;
+
+ public static final int kShooterMotorPort = 4;
+ public static final int kFeederMotorPort = 5;
+
+ public static final double kShooterFreeRPS = 5300;
+ public static final double kShooterTargetRPS = 4000;
+ public static final double kShooterToleranceRPS = 50;
+
+ public static final double kP = 1;
+ public static final double kI = 0;
+ public static final double kD = 0;
+
+ // On a real robot the feedforward constants should be empirically determined; these are
+ // reasonable guesses.
+ public static final double kSFractional = .05;
+ public static final double kVFractional =
+ // Should have value 1 at free speed...
+ 1. / kShooterFreeRPS;
+
+ public static final double kFeederSpeed = .5;
+ }
+
+ public static final class AutoConstants {
+ public static final double kAutoTimeoutSeconds = 12;
+ public static final double kAutoShootTimeSeconds = 7;
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 1;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java
similarity index 76%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java
index 787aff0..1fdb348 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.frisbeebot;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java
new file mode 100644
index 0000000..83a289d
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.frisbeebot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
new file mode 100644
index 0000000..ea6377a
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.frisbeebot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.ShooterSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants.kAutoShootTimeSeconds;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants.kAutoTimeoutSeconds;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems
+ private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+ private final ShooterSubsystem m_shooter = new ShooterSubsystem();
+
+ // A simple autonomous routine that shoots the loaded frisbees
+ private final Command m_autoCommand =
+ // Start the command by spinning up the shooter...
+ new InstantCommand(m_shooter::enable, m_shooter).andThen(
+ // Wait until the shooter is at speed before feeding the frisbees
+ new WaitUntilCommand(m_shooter::atSetpoint),
+ // Start running the feeder
+ new InstantCommand(m_shooter::runFeeder, m_shooter),
+ // Shoot for the specified time
+ new WaitCommand(kAutoShootTimeSeconds))
+ // Add a timeout (will end the command if, for instance, the shooter never gets up to
+ // speed)
+ .withTimeout(kAutoTimeoutSeconds)
+ // When the command ends, turn off the shooter and the feeder
+ .andThen(() -> {
+ m_shooter.disable();
+ m_shooter.stopFeeder();
+ });
+
+ // The driver's controller
+ XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+
+ // Configure default commands
+ // Set the default drive command to split-stick arcade drive
+ m_robotDrive.setDefaultCommand(
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ new RunCommand(() -> m_robotDrive
+ .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
+ m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
+
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+ * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ // Spin up the shooter when the 'A' button is pressed
+ new JoystickButton(m_driverController, Button.kA.value)
+ .whenPressed(new InstantCommand(m_shooter::enable, m_shooter));
+
+ // Turn off the shooter when the 'B' button is pressed
+ new JoystickButton(m_driverController, Button.kB.value)
+ .whenPressed(new InstantCommand(m_shooter::disable, m_shooter));
+
+ // Run the feeder when the 'X' button is held, but only if the shooter is at speed
+ new JoystickButton(m_driverController, Button.kX.value).whenPressed(new ConditionalCommand(
+ // Run the feeder
+ new InstantCommand(m_shooter::runFeeder, m_shooter),
+ // Do nothing
+ new InstantCommand(),
+ // Determine which of the above to do based on whether the shooter has reached the
+ // desired speed
+ m_shooter::atSetpoint)).whenReleased(new InstantCommand(m_shooter::stopFeeder, m_shooter));
+
+ // Drive at half speed when the bumper is held
+ new JoystickButton(m_driverController, Button.kBumperRight.value)
+ .whenPressed(() -> m_robotDrive.setMaxOutput(.5))
+ .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+ }
+
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ return m_autoCommand;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000..c9d8729
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftMotor1Port;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftMotor2Port;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightMotor1Port;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightMotor2Port;
+
+public class DriveSubsystem extends SubsystemBase {
+ // The motors on the left side of the drive.
+ private final SpeedControllerGroup m_leftMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
+ new PWMVictorSPX(kLeftMotor2Port));
+
+ // The motors on the right side of the drive.
+ private final SpeedControllerGroup m_rightMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
+ new PWMVictorSPX(kRightMotor2Port));
+
+ // The robot's drive
+ private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+
+ // The left-side drive encoder
+ private final Encoder m_leftEncoder =
+ new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
+
+ // The right-side drive encoder
+ private final Encoder m_rightEncoder =
+ new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+
+ /**
+ * Creates a new DriveSubsystem.
+ */
+ public DriveSubsystem() {
+ // Sets the distance per pulse for the encoders
+ m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ }
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ public void arcadeDrive(double fwd, double rot) {
+ m_drive.arcadeDrive(fwd, rot);
+ }
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ public void resetEncoders() {
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+ }
+
+ /**
+ * Gets the average distance of the two encoders.
+ *
+ * @return the average of the two encoder readings
+ */
+ public double getAverageEncoderDistance() {
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+ }
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ public Encoder getLeftEncoder() {
+ return m_leftEncoder;
+ }
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ public Encoder getRightEncoder() {
+ return m_rightEncoder;
+ }
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_drive.setMaxOutput(maxOutput);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
new file mode 100644
index 0000000..5ebfb04
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.PIDSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kD;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kFeederMotorPort;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kFeederSpeed;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kI;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kP;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kSFractional;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterMotorPort;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterTargetRPS;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterToleranceRPS;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kVFractional;
+
+public class ShooterSubsystem extends PIDSubsystem {
+ private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(kShooterMotorPort);
+ private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(kFeederMotorPort);
+ private final Encoder m_shooterEncoder =
+ new Encoder(kEncoderPorts[0], kEncoderPorts[1], kEncoderReversed);
+
+ /**
+ * The shooter subsystem for the robot.
+ */
+ public ShooterSubsystem() {
+ super(new PIDController(kP, kI, kD));
+ getController().setTolerance(kShooterToleranceRPS);
+ m_shooterEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ }
+
+ @Override
+ public void useOutput(double output) {
+ // Use a feedforward of the form kS + kV * velocity
+ m_shooterMotor.set(output + kSFractional + kVFractional * kShooterTargetRPS);
+ }
+
+ @Override
+ public double getSetpoint() {
+ return kShooterTargetRPS;
+ }
+
+ @Override
+ public double getMeasurement() {
+ return m_shooterEncoder.getRate();
+ }
+
+ public boolean atSetpoint() {
+ return m_controller.atSetpoint();
+ }
+
+ public void runFeeder() {
+ m_feederMotor.set(kFeederSpeed);
+ }
+
+ public void stopFeeder() {
+ m_feederMotor.set(0);
+ }
+
+ @Override
+ public void disable() {
+ super.disable();
+ // Turn off motor when we disable, since useOutput(0) doesn't stop the motor due to our
+ // feedforward
+ m_shooterMotor.set(0);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java
index 02a1475..60aea50 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,9 +10,9 @@
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/OI.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/OI.java
deleted file mode 100644
index 2a2469f..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/OI.java
+++ /dev/null
@@ -1,72 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.gearsbot;
-
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.buttons.JoystickButton;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-import edu.wpi.first.wpilibj.examples.gearsbot.commands.Autonomous;
-import edu.wpi.first.wpilibj.examples.gearsbot.commands.CloseClaw;
-import edu.wpi.first.wpilibj.examples.gearsbot.commands.OpenClaw;
-import edu.wpi.first.wpilibj.examples.gearsbot.commands.Pickup;
-import edu.wpi.first.wpilibj.examples.gearsbot.commands.Place;
-import edu.wpi.first.wpilibj.examples.gearsbot.commands.PrepareToPickup;
-import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetElevatorSetpoint;
-import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetWristSetpoint;
-
-/**
- * This class is the glue that binds the controls on the physical operator
- * interface to the commands and command groups that allow control of the robot.
- */
-public class OI {
- private final Joystick m_joystick = new Joystick(0);
-
- /**
- * Construct the OI and all of the buttons on it.
- */
- public OI() {
- // Put Some buttons on the SmartDashboard
- SmartDashboard.putData("Elevator Bottom", new SetElevatorSetpoint(0));
- SmartDashboard.putData("Elevator Platform", new SetElevatorSetpoint(0.2));
- SmartDashboard.putData("Elevator Top", new SetElevatorSetpoint(0.3));
-
- SmartDashboard.putData("Wrist Horizontal", new SetWristSetpoint(0));
- SmartDashboard.putData("Raise Wrist", new SetWristSetpoint(-45));
-
- SmartDashboard.putData("Open Claw", new OpenClaw());
- SmartDashboard.putData("Close Claw", new CloseClaw());
-
- SmartDashboard.putData("Deliver Soda", new Autonomous());
-
- // Create some buttons
- final JoystickButton dpadUp = new JoystickButton(m_joystick, 5);
- final JoystickButton dpadRight = new JoystickButton(m_joystick, 6);
- final JoystickButton dpadDown = new JoystickButton(m_joystick, 7);
- final JoystickButton dpadLeft = new JoystickButton(m_joystick, 8);
- final JoystickButton l2 = new JoystickButton(m_joystick, 9);
- final JoystickButton r2 = new JoystickButton(m_joystick, 10);
- final JoystickButton l1 = new JoystickButton(m_joystick, 11);
- final JoystickButton r1 = new JoystickButton(m_joystick, 12);
-
- // Connect the buttons to commands
- dpadUp.whenPressed(new SetElevatorSetpoint(0.2));
- dpadDown.whenPressed(new SetElevatorSetpoint(-0.2));
- dpadRight.whenPressed(new CloseClaw());
- dpadLeft.whenPressed(new OpenClaw());
-
- r1.whenPressed(new PrepareToPickup());
- r2.whenPressed(new Pickup());
- l1.whenPressed(new Place());
- l2.whenPressed(new Autonomous());
- }
-
- public Joystick getJoystick() {
- return m_joystick;
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java
index d82139f..ea26e28 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,58 +8,69 @@
package edu.wpi.first.wpilibj.examples.gearsbot;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.command.Scheduler;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-import edu.wpi.first.wpilibj.examples.gearsbot.commands.Autonomous;
-import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
-import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
-import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
-import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the TimedRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the manifest file in the resource
- * directory.
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
*/
public class Robot extends TimedRobot {
- Command m_autonomousCommand;
+ private Command m_autonomousCommand;
- public static DriveTrain m_drivetrain;
- public static Elevator m_elevator;
- public static Wrist m_wrist;
- public static Claw m_claw;
- public static OI m_oi;
+ private RobotContainer m_robotContainer;
/**
- * This function is run when the robot is first started up and should be
- * used for any initialization code.
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
*/
@Override
public void robotInit() {
- // Initialize all subsystems
- m_drivetrain = new DriveTrain();
- m_elevator = new Elevator();
- m_wrist = new Wrist();
- m_claw = new Claw();
- m_oi = new OI();
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
- // instantiate the command used for the autonomous period
- m_autonomousCommand = new Autonomous();
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
- // Show what command your subsystem is running on the SmartDashboard
- SmartDashboard.putData(m_drivetrain);
- SmartDashboard.putData(m_elevator);
- SmartDashboard.putData(m_wrist);
- SmartDashboard.putData(m_claw);
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
}
@Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
public void autonomousInit() {
- m_autonomousCommand.start(); // schedule the autonomous command (example)
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
}
/**
@@ -67,8 +78,6 @@
*/
@Override
public void autonomousPeriodic() {
- Scheduler.getInstance().run();
- log();
}
@Override
@@ -77,16 +86,22 @@
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
- m_autonomousCommand.cancel();
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
}
/**
- * This function is called periodically during teleoperated mode.
+ * This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
- Scheduler.getInstance().run();
- log();
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
}
/**
@@ -95,14 +110,4 @@
@Override
public void testPeriodic() {
}
-
- /**
- * The log method puts interesting information to the SmartDashboard.
- */
- private void log() {
- m_wrist.log();
- m_elevator.log();
- m_drivetrain.log();
- m_claw.log();
- }
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java
new file mode 100644
index 0000000..69eb8ae
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java
@@ -0,0 +1,127 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.GenericHID.Hand;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.Autonomous;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.CloseClaw;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.OpenClaw;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.Pickup;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.Place;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.PrepareToPickup;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetElevatorSetpoint;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetWristSetpoint;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.TankDrive;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems and commands are defined here...
+ private final DriveTrain m_drivetrain = new DriveTrain();
+ private final Elevator m_elevator = new Elevator();
+ private final Wrist m_wrist = new Wrist();
+ private final Claw m_claw = new Claw();
+
+ private final Joystick m_joystick = new Joystick(0);
+
+ private final CommandBase m_autonomousCommand =
+ new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Put Some buttons on the SmartDashboard
+ SmartDashboard.putData("Elevator Bottom", new SetElevatorSetpoint(0, m_elevator));
+ SmartDashboard.putData("Elevator Platform", new SetElevatorSetpoint(0.2, m_elevator));
+ SmartDashboard.putData("Elevator Top", new SetElevatorSetpoint(0.3, m_elevator));
+
+ SmartDashboard.putData("Wrist Horizontal", new SetWristSetpoint(0, m_wrist));
+ SmartDashboard.putData("Raise Wrist", new SetWristSetpoint(-45, m_wrist));
+
+ SmartDashboard.putData("Open Claw", new OpenClaw(m_claw));
+ SmartDashboard.putData("Close Claw", new CloseClaw(m_claw));
+
+ SmartDashboard
+ .putData("Deliver Soda", new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator));
+
+ // Assign default commands
+ m_drivetrain.setDefaultCommand(new TankDrive(() -> m_joystick.getY(Hand.kLeft),
+ () -> m_joystick.getY(Hand.kRight), m_drivetrain));
+
+ // Show what command your subsystem is running on the SmartDashboard
+ SmartDashboard.putData(m_drivetrain);
+ SmartDashboard.putData(m_elevator);
+ SmartDashboard.putData(m_wrist);
+ SmartDashboard.putData(m_claw);
+
+ // Call log method on all subsystems
+ m_wrist.log();
+ m_elevator.log();
+ m_drivetrain.log();
+ m_claw.log();
+
+ // Configure the button bindings
+ configureButtonBindings();
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+ * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ // Create some buttons
+ final JoystickButton dpadUp = new JoystickButton(m_joystick, 5);
+ final JoystickButton dpadRight = new JoystickButton(m_joystick, 6);
+ final JoystickButton dpadDown = new JoystickButton(m_joystick, 7);
+ final JoystickButton dpadLeft = new JoystickButton(m_joystick, 8);
+ final JoystickButton l2 = new JoystickButton(m_joystick, 9);
+ final JoystickButton r2 = new JoystickButton(m_joystick, 10);
+ final JoystickButton l1 = new JoystickButton(m_joystick, 11);
+ final JoystickButton r1 = new JoystickButton(m_joystick, 12);
+
+ // Connect the buttons to commands
+ dpadUp.whenPressed(new SetElevatorSetpoint(0.2, m_elevator));
+ dpadDown.whenPressed(new SetElevatorSetpoint(-0.2, m_elevator));
+ dpadRight.whenPressed(new CloseClaw(m_claw));
+ dpadLeft.whenPressed(new OpenClaw(m_claw));
+
+ r1.whenPressed(new PrepareToPickup(m_claw, m_wrist, m_elevator));
+ r2.whenPressed(new Pickup(m_claw, m_wrist, m_elevator));
+ l1.whenPressed(new Place(m_claw, m_wrist, m_elevator));
+ l2.whenPressed(new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator));
+ }
+
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ return m_autonomousCommand;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java
index 96a9d85..0f0db70 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,26 +7,33 @@
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
-import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
/**
* The main autonomous command to pickup and deliver the soda to the box.
*/
-public class Autonomous extends CommandGroup {
+public class Autonomous extends SequentialCommandGroup {
/**
* Create a new autonomous command.
*/
- public Autonomous() {
- addSequential(new PrepareToPickup());
- addSequential(new Pickup());
- addSequential(new SetDistanceToBox(0.10));
- // addSequential(new DriveStraight(4)); // Use Encoders if ultrasonic is
- // broken
- addSequential(new Place());
- addSequential(new SetDistanceToBox(0.60));
- // addSequential(new DriveStraight(-2)); // Use Encoders if ultrasonic
- // is broken
- addParallel(new SetWristSetpoint(-45));
- addSequential(new CloseClaw());
+ public Autonomous(DriveTrain drive, Claw claw, Wrist wrist, Elevator elevator) {
+ addCommands(
+ new PrepareToPickup(claw, wrist, elevator),
+ new Pickup(claw, wrist, elevator),
+ new SetDistanceToBox(0.10, drive),
+ // new DriveStraight(4), // Use encoders if ultrasonic is broken
+ new Place(claw, wrist, elevator),
+ new SetDistanceToBox(0.60, drive),
+ // new DriveStraight(-2), // Use Encoders if ultrasonic is broken
+ parallel(
+ new SetWristSetpoint(-45, wrist),
+ new CloseClaw(claw)
+ )
+ );
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java
index cdf4788..0ebce9a 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,40 +7,43 @@
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
-import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
/**
- * Closes the claw for one second. Real robots should use sensors, stalling
- * motors is BAD!
+ * Closes the claw for one second. Real robots should use sensors, stalling motors is BAD!
*/
-public class CloseClaw extends Command {
- public CloseClaw() {
- requires(Robot.m_claw);
+public class CloseClaw extends CommandBase {
+ private final Claw m_claw;
+
+ public CloseClaw(Claw claw) {
+ m_claw = claw;
+ addRequirements(m_claw);
}
// Called just before this Command runs the first time
@Override
- protected void initialize() {
- Robot.m_claw.close();
+ public void initialize() {
+ m_claw.close();
}
// Make this return true when this Command no longer needs to run execute()
@Override
- protected boolean isFinished() {
- return Robot.m_claw.isGrabbing();
+ public boolean isFinished() {
+ return m_claw.isGrabbing();
}
// Called once after isFinished returns true
@Override
- protected void end() {
+ public void end(boolean interrupted) {
// NOTE: Doesn't stop in simulation due to lower friction causing the
// can to fall out
// + there is no need to worry about stalling the motor or crushing the
// can.
if (!Robot.isSimulation()) {
- Robot.m_claw.stop();
+ m_claw.stop();
}
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java
index ba8feeb..afed30d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,12 +7,10 @@
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
-import edu.wpi.first.wpilibj.PIDController;
-import edu.wpi.first.wpilibj.PIDSource;
-import edu.wpi.first.wpilibj.PIDSourceType;
-import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
-import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
/**
* Drive the given distance straight (negative values go backwards). Uses a
@@ -20,58 +18,36 @@
* command is running. The input is the averaged values of the left and right
* encoders.
*/
-public class DriveStraight extends Command {
- private final PIDController m_pid;
+public class DriveStraight extends PIDCommand {
+ private final DriveTrain m_drivetrain;
/**
* Create a new DriveStraight command.
* @param distance The distance to drive
*/
- public DriveStraight(double distance) {
- requires(Robot.m_drivetrain);
- m_pid = new PIDController(4, 0, 0, new PIDSource() {
- PIDSourceType m_sourceType = PIDSourceType.kDisplacement;
+ public DriveStraight(double distance, DriveTrain drivetrain) {
+ super(new PIDController(4, 0, 0),
+ drivetrain::getDistance,
+ distance,
+ d -> drivetrain.drive(d, d));
- @Override
- public double pidGet() {
- return Robot.m_drivetrain.getDistance();
- }
+ m_drivetrain = drivetrain;
+ addRequirements(m_drivetrain);
- @Override
- public void setPIDSourceType(PIDSourceType pidSource) {
- m_sourceType = pidSource;
- }
-
- @Override
- public PIDSourceType getPIDSourceType() {
- return m_sourceType;
- }
- }, d -> Robot.m_drivetrain.drive(d, d));
-
- m_pid.setAbsoluteTolerance(0.01);
- m_pid.setSetpoint(distance);
+ getController().setTolerance(0.01);
}
// Called just before this Command runs the first time
@Override
- protected void initialize() {
+ public void initialize() {
// Get everything in a safe starting state.
- Robot.m_drivetrain.reset();
- m_pid.reset();
- m_pid.enable();
+ m_drivetrain.reset();
+ super.initialize();
}
// Make this return true when this Command no longer needs to run execute()
@Override
- protected boolean isFinished() {
- return m_pid.onTarget();
- }
-
- // Called once after isFinished returns true
- @Override
- protected void end() {
- // Stop PID and the wheels
- m_pid.disable();
- Robot.m_drivetrain.drive(0, 0);
+ public boolean isFinished() {
+ return getController().atSetpoint();
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java
index 486181d..9d944b4 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,29 +7,37 @@
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
-import edu.wpi.first.wpilibj.command.TimedCommand;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
-import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
/**
- * Opens the claw for one second. Real robots should use sensors, stalling
- * motors is BAD!
+ * Opens the claw for one second. Real robots should use sensors, stalling motors is BAD!
*/
-public class OpenClaw extends TimedCommand {
- public OpenClaw() {
+public class OpenClaw extends WaitCommand {
+ private final Claw m_claw;
+
+ /**
+ * Creates a new OpenClaw command.
+ *
+ * @param claw The claw to use
+ */
+ public OpenClaw(Claw claw) {
super(1);
- requires(Robot.m_claw);
+ m_claw = claw;
+ addRequirements(m_claw);
}
// Called just before this Command runs the first time
@Override
- protected void initialize() {
- Robot.m_claw.open();
+ public void initialize() {
+ m_claw.open();
+ super.initialize();
}
// Called once after isFinished returns true
@Override
- protected void end() {
- Robot.m_claw.stop();
+ public void end(boolean interrupted) {
+ m_claw.stop();
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java
index 304ddf9..b51d106 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,19 +7,29 @@
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
-import edu.wpi.first.wpilibj.command.CommandGroup;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
/**
- * Pickup a soda can (if one is between the open claws) and get it in a safe
- * state to drive around.
+ * Pickup a soda can (if one is between the open claws) and get it in a safe state to drive around.
*/
-public class Pickup extends CommandGroup {
+public class Pickup extends SequentialCommandGroup {
/**
* Create a new pickup command.
+ *
+ * @param claw The claw subsystem to use
+ * @param wrist The wrist subsystem to use
+ * @param elevator The elevator subsystem to use
*/
- public Pickup() {
- addSequential(new CloseClaw());
- addParallel(new SetWristSetpoint(-45));
- addSequential(new SetElevatorSetpoint(0.25));
+ public Pickup(Claw claw, Wrist wrist, Elevator elevator) {
+ addCommands(
+ new CloseClaw(claw),
+ parallel(
+ new SetWristSetpoint(-45, wrist),
+ new SetElevatorSetpoint(0.25, elevator)));
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java
index c1a1841..c57ffe2 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,18 +7,28 @@
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
-import edu.wpi.first.wpilibj.command.CommandGroup;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
/**
* Place a held soda can onto the platform.
*/
-public class Place extends CommandGroup {
+public class Place extends SequentialCommandGroup {
/**
* Create a new place command.
+ *
+ * @param claw The claw subsystem to use
+ * @param wrist The wrist subsystem to use
+ * @param elevator The elevator subsystem to use
*/
- public Place() {
- addSequential(new SetElevatorSetpoint(0.25));
- addSequential(new SetWristSetpoint(0));
- addSequential(new OpenClaw());
+ public Place(Claw claw, Wrist wrist, Elevator elevator) {
+ addCommands(
+ new SetElevatorSetpoint(0.25, elevator),
+ new SetWristSetpoint(0, wrist),
+ new OpenClaw(claw));
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java
index 911c535..620c184 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,18 +7,28 @@
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
-import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
/**
* Make sure the robot is in a state to pickup soda cans.
*/
-public class PrepareToPickup extends CommandGroup {
+public class PrepareToPickup extends SequentialCommandGroup {
/**
* Create a new prepare to pickup command.
+ *
+ * @param claw The claw subsystem to use
+ * @param wrist The wrist subsystem to use
+ * @param elevator The elevator subsystem to use
*/
- public PrepareToPickup() {
- addParallel(new OpenClaw());
- addParallel(new SetWristSetpoint(0));
- addSequential(new SetElevatorSetpoint(0));
+ public PrepareToPickup(Claw claw, Wrist wrist, Elevator elevator) {
+ addCommands(
+ new OpenClaw(claw),
+ parallel(
+ new SetWristSetpoint(0, wrist),
+ new SetElevatorSetpoint(0, elevator)));
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java
index 71f03dc..b14ddc9 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,12 +7,11 @@
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
-import edu.wpi.first.wpilibj.PIDController;
-import edu.wpi.first.wpilibj.PIDSource;
-import edu.wpi.first.wpilibj.PIDSourceType;
-import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
-import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
+
/**
* Drive until the robot is the given distance away from the box. Uses a local
@@ -20,58 +19,36 @@
* command is running. The input is the averaged values of the left and right
* encoders.
*/
-public class SetDistanceToBox extends Command {
- private final PIDController m_pid;
+public class SetDistanceToBox extends PIDCommand {
+ private final DriveTrain m_drivetrain;
/**
* Create a new set distance to box command.
+ *
* @param distance The distance away from the box to drive to
*/
- public SetDistanceToBox(double distance) {
- requires(Robot.m_drivetrain);
- m_pid = new PIDController(-2, 0, 0, new PIDSource() {
- PIDSourceType m_sourceType = PIDSourceType.kDisplacement;
+ public SetDistanceToBox(double distance, DriveTrain drivetrain) {
+ super(new PIDController(-2, 0, 0),
+ drivetrain::getDistanceToObstacle, distance,
+ d -> drivetrain.drive(d, d));
- @Override
- public double pidGet() {
- return Robot.m_drivetrain.getDistanceToObstacle();
- }
+ m_drivetrain = drivetrain;
+ addRequirements(m_drivetrain);
- @Override
- public void setPIDSourceType(PIDSourceType pidSource) {
- m_sourceType = pidSource;
- }
-
- @Override
- public PIDSourceType getPIDSourceType() {
- return m_sourceType;
- }
- }, d -> Robot.m_drivetrain.drive(d, d));
-
- m_pid.setAbsoluteTolerance(0.01);
- m_pid.setSetpoint(distance);
+ getController().setTolerance(0.01);
}
// Called just before this Command runs the first time
@Override
- protected void initialize() {
+ public void initialize() {
// Get everything in a safe starting state.
- Robot.m_drivetrain.reset();
- m_pid.reset();
- m_pid.enable();
+ m_drivetrain.reset();
+ super.initialize();
}
// Make this return true when this Command no longer needs to run execute()
@Override
- protected boolean isFinished() {
- return m_pid.onTarget();
- }
-
- // Called once after isFinished returns true
- @Override
- protected void end() {
- // Stop PID and the wheels
- m_pid.disable();
- Robot.m_drivetrain.drive(0, 0);
+ public boolean isFinished() {
+ return getController().atSetpoint();
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java
index 0ee5193..44c6677 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,34 +7,42 @@
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
-import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
-import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
+
/**
- * Move the elevator to a given location. This command finishes when it is
- * within the tolerance, but leaves the PID loop running to maintain the
- * position. Other commands using the elevator should make sure they disable
- * PID!
+ * Move the elevator to a given location. This command finishes when it is within the tolerance, but
+ * leaves the PID loop running to maintain the position. Other commands using the elevator should
+ * make sure they disable PID!
*/
-public class SetElevatorSetpoint extends Command {
+public class SetElevatorSetpoint extends CommandBase {
+ private final Elevator m_elevator;
private final double m_setpoint;
- public SetElevatorSetpoint(double setpoint) {
+ /**
+ * Create a new SetElevatorSetpoint command.
+ *
+ * @param setpoint The setpoint to set the elevator to
+ * @param elevator The elevator to use
+ */
+ public SetElevatorSetpoint(double setpoint, Elevator elevator) {
+ m_elevator = elevator;
m_setpoint = setpoint;
- requires(Robot.m_elevator);
+ addRequirements(m_elevator);
}
// Called just before this Command runs the first time
@Override
- protected void initialize() {
- Robot.m_elevator.enable();
- Robot.m_elevator.setSetpoint(m_setpoint);
+ public void initialize() {
+ m_elevator.setSetpoint(m_setpoint);
+ m_elevator.enable();
}
// Make this return true when this Command no longer needs to run execute()
@Override
- protected boolean isFinished() {
- return Robot.m_elevator.onTarget();
+ public boolean isFinished() {
+ return m_elevator.getController().atSetpoint();
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java
index 62e7a7f..d03211a 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,33 +7,42 @@
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
-import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
-import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
+
/**
- * Move the wrist to a given angle. This command finishes when it is within the
- * tolerance, but leaves the PID loop running to maintain the position. Other
- * commands using the wrist should make sure they disable PID!
+ * Move the wrist to a given angle. This command finishes when it is within the tolerance, but
+ * leaves the PID loop running to maintain the position. Other commands using the wrist should make
+ * sure they disable PID!
*/
-public class SetWristSetpoint extends Command {
+public class SetWristSetpoint extends CommandBase {
+ private final Wrist m_wrist;
private final double m_setpoint;
- public SetWristSetpoint(double setpoint) {
+ /**
+ * Create a new SetWristSetpoint command.
+ *
+ * @param setpoint The setpoint to set the wrist to
+ * @param wrist The wrist to use
+ */
+ public SetWristSetpoint(double setpoint, Wrist wrist) {
+ m_wrist = wrist;
m_setpoint = setpoint;
- requires(Robot.m_wrist);
+ addRequirements(m_wrist);
}
// Called just before this Command runs the first time
@Override
- protected void initialize() {
- Robot.m_wrist.enable();
- Robot.m_wrist.setSetpoint(m_setpoint);
+ public void initialize() {
+ m_wrist.enable();
+ m_wrist.setSetpoint(m_setpoint);
}
// Make this return true when this Command no longer needs to run execute()
@Override
- protected boolean isFinished() {
- return Robot.m_wrist.onTarget();
+ public boolean isFinished() {
+ return m_wrist.getController().atSetpoint();
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDrive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDrive.java
new file mode 100644
index 0000000..b5c24f2
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDrive.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot.commands;
+
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
+
+/**
+ * Have the robot drive tank style.
+ */
+public class TankDrive extends CommandBase {
+ private final DriveTrain m_drivetrain;
+ private final DoubleSupplier m_left;
+ private final DoubleSupplier m_right;
+
+ /**
+ * Creates a new TankDrive command.
+ *
+ * @param left The control input for the left side of the drive
+ * @param right The control input for the right sight of the drive
+ * @param drivetrain The drivetrain subsystem to drive
+ */
+ public TankDrive(DoubleSupplier left, DoubleSupplier right, DriveTrain drivetrain) {
+ m_drivetrain = drivetrain;
+ m_left = left;
+ m_right = right;
+ addRequirements(m_drivetrain);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ public void execute() {
+ m_drivetrain.drive(m_left.getAsDouble(), m_right.getAsDouble());
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ public boolean isFinished() {
+ return false; // Runs until interrupted
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ public void end(boolean interrupted) {
+ m_drivetrain.drive(0, 0);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDriveWithJoystick.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDriveWithJoystick.java
deleted file mode 100644
index 642fa5d..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDriveWithJoystick.java
+++ /dev/null
@@ -1,39 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.gearsbot.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
-
-/**
- * Have the robot drive tank style using the PS3 Joystick until interrupted.
- */
-public class TankDriveWithJoystick extends Command {
- public TankDriveWithJoystick() {
- requires(Robot.m_drivetrain);
- }
-
- // Called repeatedly when this Command is scheduled to run
- @Override
- protected void execute() {
- Robot.m_drivetrain.drive(Robot.m_oi.getJoystick());
- }
-
- // Make this return true when this Command no longer needs to run execute()
- @Override
- protected boolean isFinished() {
- return false; // Runs until interrupted
- }
-
- // Called once after isFinished returns true
- @Override
- protected void end() {
- Robot.m_drivetrain.drive(0, 0);
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java
index 550db9a..b0d2515 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,14 +9,14 @@
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Victor;
-import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
/**
- * The claw subsystem is a simple system with a motor for opening and closing.
- * If using stronger motors, you should probably use a sensor so that the motors
- * don't stall.
+ * The claw subsystem is a simple system with a motor for opening and closing. If using stronger
+ * motors, you should probably use a sensor so that the motors don't stall.
*/
-public class Claw extends Subsystem {
+public class Claw extends SubsystemBase {
private final Victor m_motor = new Victor(7);
private final DigitalInput m_contact = new DigitalInput(5);
@@ -24,18 +24,13 @@
* Create a new claw subsystem.
*/
public Claw() {
- super();
-
// Let's name everything on the LiveWindow
addChild("Motor", m_motor);
addChild("Limit Switch", m_contact);
}
- @Override
- public void initDefaultCommand() {
- }
-
public void log() {
+ SmartDashboard.putData("Claw switch", m_contact);
}
/**
@@ -48,7 +43,6 @@
/**
* Set the claw motor to move in the close direction.
*/
- @Override
public void close() {
m_motor.set(1);
}
@@ -61,8 +55,7 @@
}
/**
- * Return true when the robot is grabbing an object hard enough to trigger
- * the limit switch.
+ * Return true when the robot is grabbing an object hard enough to trigger the limit switch.
*/
public boolean isGrabbing() {
return m_contact.get();
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java
index 5c55365..a0aaa32 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,30 +10,26 @@
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
-import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
-import edu.wpi.first.wpilibj.examples.gearsbot.commands.TankDriveWithJoystick;
-/**
- * The DriveTrain subsystem incorporates the sensors and actuators attached to
- * the robots chassis. These include four drive motors, a left and right encoder
- * and a gyro.
- */
-public class DriveTrain extends Subsystem {
- private final SpeedController m_leftMotor
- = new SpeedControllerGroup(new PWMVictorSPX(0), new PWMVictorSPX(1));
- private final SpeedController m_rightMotor
- = new SpeedControllerGroup(new PWMVictorSPX(2), new PWMVictorSPX(3));
+public class DriveTrain extends SubsystemBase {
+ /**
+ * The DriveTrain subsystem incorporates the sensors and actuators attached to the robots chassis.
+ * These include four drive motors, a left and right encoder and a gyro.
+ */
+ private final SpeedController m_leftMotor =
+ new SpeedControllerGroup(new PWMVictorSPX(0), new PWMVictorSPX(1));
+ private final SpeedController m_rightMotor =
+ new SpeedControllerGroup(new PWMVictorSPX(2), new PWMVictorSPX(3));
- private final DifferentialDrive m_drive
- = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotor, m_rightMotor);
private final Encoder m_leftEncoder = new Encoder(1, 2);
private final Encoder m_rightEncoder = new Encoder(3, 4);
@@ -69,15 +65,6 @@
}
/**
- * When no other command is running let the operator drive around using the
- * PS3 joystick.
- */
- @Override
- public void initDefaultCommand() {
- setDefaultCommand(new TankDriveWithJoystick());
- }
-
- /**
* The log method puts interesting information to the SmartDashboard.
*/
public void log() {
@@ -91,7 +78,7 @@
/**
* Tank style driving for the DriveTrain.
*
- * @param left Speed in range [-1,1]
+ * @param left Speed in range [-1,1]
* @param right Speed in range [-1,1]
*/
public void drive(double left, double right) {
@@ -99,15 +86,6 @@
}
/**
- * Tank style driving for the DriveTrain.
- *
- * @param joy The ps3 style joystick to use to drive tank style.
- */
- public void drive(Joystick joy) {
- drive(-joy.getY(), -joy.getThrottle());
- }
-
- /**
* Get the robot's heading.
*
* @return The robots heading in degrees.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
index 02d308c..fffd478 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,19 +9,20 @@
import edu.wpi.first.wpilibj.AnalogPotentiometer;
import edu.wpi.first.wpilibj.Victor;
-import edu.wpi.first.wpilibj.command.PIDSubsystem;
+import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.PIDSubsystem;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
/**
- * The elevator subsystem uses PID to go to a given height. Unfortunately, in
- * it's current state PID values for simulation are different than in the real
- * world do to minor differences.
+ * The elevator subsystem uses PID to go to a given height. Unfortunately, in it's current state PID
+ * values for simulation are different than in the real world do to minor differences.
*/
public class Elevator extends PIDSubsystem {
private final Victor m_motor;
private final AnalogPotentiometer m_pot;
+ private double m_setpoint;
private static final double kP_real = 4;
private static final double kI_real = 0.07;
@@ -32,11 +33,11 @@
* Create a new elevator subsystem.
*/
public Elevator() {
- super(kP_real, kI_real, 0);
+ super(new PIDController(kP_real, kI_real, 0));
if (Robot.isSimulation()) { // Check for simulation and update PID values
- getPIDController().setPID(kP_simulation, kI_simulation, 0, 0);
+ getController().setPID(kP_simulation, kI_simulation, 0);
}
- setAbsoluteTolerance(0.005);
+ getController().setTolerance(0.005);
m_motor = new Victor(5);
@@ -53,32 +54,45 @@
addChild("Pot", m_pot);
}
- @Override
- public void initDefaultCommand() {
- }
-
/**
* The log method puts interesting information to the SmartDashboard.
*/
public void log() {
- SmartDashboard.putData("Elevator Pot", (AnalogPotentiometer) m_pot);
+ SmartDashboard.putData("Elevator Pot", m_pot);
}
/**
- * Use the potentiometer as the PID sensor. This method is automatically
- * called by the subsystem.
+ * Use the potentiometer as the PID sensor. This method is automatically called by the subsystem.
*/
@Override
- protected double returnPIDInput() {
+ public double getMeasurement() {
return m_pot.get();
}
/**
- * Use the motor as the PID output. This method is automatically called by
- * the subsystem.
+ * Use the motor as the PID output. This method is automatically called by the subsystem.
*/
@Override
- protected void usePIDOutput(double power) {
- m_motor.set(power);
+ public void useOutput(double output) {
+ m_motor.set(output);
+ }
+
+ /**
+ * Returns the setpoint used by the PIDController.
+ *
+ * @return The setpoint for the PIDController.
+ */
+ @Override
+ public double getSetpoint() {
+ return m_setpoint;
+ }
+
+ /**
+ * Sets the setpoint used by the PIDController.
+ *
+ * @param setpoint The setpoint for the PIDController.
+ */
+ public void setSetpoint(double setpoint) {
+ m_setpoint = setpoint;
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
index 30fa38e..ae65bed 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,18 +9,19 @@
import edu.wpi.first.wpilibj.AnalogPotentiometer;
import edu.wpi.first.wpilibj.Victor;
-import edu.wpi.first.wpilibj.command.PIDSubsystem;
+import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.PIDSubsystem;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
/**
- * The wrist subsystem is like the elevator, but with a rotational joint instead
- * of a linear joint.
+ * The wrist subsystem is like the elevator, but with a rotational joint instead of a linear joint.
*/
public class Wrist extends PIDSubsystem {
private final Victor m_motor;
private final AnalogPotentiometer m_pot;
+ private double m_setpoint;
private static final double kP_real = 1;
private static final double kP_simulation = 0.05;
@@ -29,11 +30,11 @@
* Create a new wrist subsystem.
*/
public Wrist() {
- super(kP_real, 0, 0);
+ super(new PIDController(kP_real, 0, 0));
if (Robot.isSimulation()) { // Check for simulation and update PID values
- getPIDController().setPID(kP_simulation, 0, 0, 0);
+ getController().setPID(kP_simulation, 0, 0);
}
- setAbsoluteTolerance(2.5);
+ getController().setTolerance(2.5);
m_motor = new Victor(6);
@@ -50,32 +51,45 @@
addChild("Pot", m_pot);
}
- @Override
- public void initDefaultCommand() {
- }
-
/**
* The log method puts interesting information to the SmartDashboard.
*/
public void log() {
- SmartDashboard.putData("Wrist Angle", (AnalogPotentiometer) m_pot);
+ SmartDashboard.putData("Wrist Angle", m_pot);
}
/**
- * Use the potentiometer as the PID sensor. This method is automatically
- * called by the subsystem.
+ * Use the potentiometer as the PID sensor. This method is automatically called by the subsystem.
*/
@Override
- protected double returnPIDInput() {
+ public double getMeasurement() {
return m_pot.get();
}
/**
- * Use the motor as the PID output. This method is automatically called by
- * the subsystem.
+ * Use the motor as the PID output. This method is automatically called by the subsystem.
*/
@Override
- protected void usePIDOutput(double power) {
- m_motor.set(power);
+ public void useOutput(double output) {
+ m_motor.set(output);
+ }
+
+ /**
+ * Returns the setpoint used by the PIDController.
+ *
+ * @return The setpoint for the PIDController.
+ */
+ @Override
+ public double getSetpoint() {
+ return m_setpoint;
+ }
+
+ /**
+ * Sets the setpoint used by the PIDController.
+ *
+ * @param setpoint The setpoint for the PIDController.
+ */
+ public void setSetpoint(double setpoint) {
+ m_setpoint = setpoint;
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java
new file mode 100644
index 0000000..50afb7b
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kLeftMotor1Port = 0;
+ public static final int kLeftMotor2Port = 1;
+ public static final int kRightMotor1Port = 2;
+ public static final int kRightMotor2Port = 3;
+
+ public static final int[] kLeftEncoderPorts = new int[]{0, 1};
+ public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final boolean kLeftEncoderReversed = false;
+ public static final boolean kRightEncoderReversed = true;
+
+ public static final int kEncoderCPR = 1024;
+ public static final double kWheelDiameterInches = 6;
+ public static final double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
+
+ public static final boolean kGyroReversed = false;
+
+ public static final double kStabilizationP = 1;
+ public static final double kStabilizationI = .5;
+ public static final double kStabilizationD = 0;
+
+ public static final double kTurnP = 1;
+ public static final double kTurnI = 0;
+ public static final double kTurnD = 0;
+
+ public static final double kTurnToleranceDeg = 5;
+ public static final double kTurnRateToleranceDegPerS = 10; // degrees per second
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 1;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java
similarity index 75%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java
index 787aff0..f18e95c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java
new file mode 100644
index 0000000..40c6db9
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
new file mode 100644
index 0000000..5a6d632
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngle;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationD;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationI;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationP;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems
+ private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+
+ // The driver's controller
+ XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+
+ // Configure default commands
+ // Set the default drive command to split-stick arcade drive
+ m_robotDrive.setDefaultCommand(
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ new RunCommand(() -> m_robotDrive
+ .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
+ m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
+
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+ * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ // Drive at half speed when the right bumper is held
+ new JoystickButton(m_driverController, Button.kBumperRight.value)
+ .whenPressed(() -> m_robotDrive.setMaxOutput(.5))
+ .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+
+ // Stabilize robot to drive straight with gyro when left bumper is held
+ new JoystickButton(m_driverController, Button.kBumperLeft.value).whenHeld(
+ new PIDCommand(
+ new PIDController(kStabilizationP, kStabilizationI, kStabilizationD),
+ // Close the loop on the turn rate
+ m_robotDrive::getTurnRate,
+ // Setpoint is 0
+ 0,
+ // Pipe the output to the turning controls
+ output -> m_robotDrive
+ .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), output),
+ // Require the robot drive
+ m_robotDrive));
+
+ // Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout
+ new JoystickButton(m_driverController, Button.kX.value)
+ .whenPressed(new TurnToAngle(90, m_robotDrive).withTimeout(5));
+ }
+
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ // no auto
+ return new InstantCommand();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java
new file mode 100644
index 0000000..fe6c725
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;
+
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnD;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnI;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnP;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnRateToleranceDegPerS;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnToleranceDeg;
+
+/**
+ * A command that will turn the robot to the specified angle.
+ */
+public class TurnToAngle extends PIDCommand {
+ /**
+ * Turns to robot to the specified angle.
+ *
+ * @param targetAngleDegrees The angle to turn to
+ * @param drive The drive subsystem to use
+ */
+ public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) {
+ super(new PIDController(kTurnP, kTurnI, kTurnD),
+ // Close loop on heading
+ drive::getHeading,
+ // Set reference to target
+ targetAngleDegrees,
+ // Pipe output to turn robot
+ output -> drive.arcadeDrive(0, output),
+ // Require the drive
+ drive);
+
+ // Set the controller to be continuous (because it is an angle controller)
+ getController().enableContinuousInput(-180, 180);
+ // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
+ // setpoint before it is considered as having reached the reference
+ getController().setTolerance(kTurnToleranceDeg, kTurnRateToleranceDegPerS);
+ }
+
+ @Override
+ public boolean isFinished() {
+ // End when the controller is at the reference.
+ return getController().atSetpoint();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000..e91b8b3
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
@@ -0,0 +1,141 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems;
+
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kGyroReversed;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftMotor1Port;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftMotor2Port;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightMotor1Port;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightMotor2Port;
+
+public class DriveSubsystem extends SubsystemBase {
+ // The motors on the left side of the drive.
+ private final SpeedControllerGroup m_leftMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
+ new PWMVictorSPX(kLeftMotor2Port));
+
+ // The motors on the right side of the drive.
+ private final SpeedControllerGroup m_rightMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
+ new PWMVictorSPX(kRightMotor2Port));
+
+ // The robot's drive
+ private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+
+ // The left-side drive encoder
+ private final Encoder m_leftEncoder =
+ new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
+
+ // The right-side drive encoder
+ private final Encoder m_rightEncoder =
+ new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+
+ // The gyro sensor
+ private final Gyro m_gyro = new ADXRS450_Gyro();
+
+ /**
+ * Creates a new DriveSubsystem.
+ */
+ public DriveSubsystem() {
+ // Sets the distance per pulse for the encoders
+ m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ }
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ public void arcadeDrive(double fwd, double rot) {
+ m_drive.arcadeDrive(fwd, rot);
+ }
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ public void resetEncoders() {
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+ }
+
+ /**
+ * Gets the average distance of the two encoders.
+ *
+ * @return the average of the two encoder readings
+ */
+ public double getAverageEncoderDistance() {
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+ }
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ public Encoder getLeftEncoder() {
+ return m_leftEncoder;
+ }
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ public Encoder getRightEncoder() {
+ return m_rightEncoder;
+ }
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_drive.setMaxOutput(maxOutput);
+ }
+
+ /**
+ * Zeroes the heading of the robot.
+ */
+ public void zeroHeading() {
+ m_gyro.reset();
+ }
+
+ /**
+ * Returns the heading of the robot.
+ *
+ * @return the robot's heading in degrees, from 180 to 180
+ */
+ public double getHeading() {
+ return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1. : 1.);
+ }
+
+ /**
+ * Returns the turn rate of the robot.
+ *
+ * @return The turn rate of the robot, in degrees per second
+ */
+ public double getTurnRate() {
+ return m_gyro.getRate() * (kGyroReversed ? -1. : 1.);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java
new file mode 100644
index 0000000..27cb872
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kLeftMotor1Port = 0;
+ public static final int kLeftMotor2Port = 1;
+ public static final int kRightMotor1Port = 2;
+ public static final int kRightMotor2Port = 3;
+
+ public static final int[] kLeftEncoderPorts = new int[]{0, 1};
+ public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final boolean kLeftEncoderReversed = false;
+ public static final boolean kRightEncoderReversed = true;
+
+ public static final int kEncoderCPR = 1024;
+ public static final double kWheelDiameterInches = 6;
+ public static final double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
+ }
+
+ public static final class HatchConstants {
+ public static final int kHatchSolenoidModule = 0;
+ public static final int[] kHatchSolenoidPorts = new int[]{0, 1};
+ }
+
+ public static final class AutoConstants {
+ public static final double kAutoDriveDistanceInches = 60;
+ public static final double kAutoBackupDistanceInches = 20;
+ public static final double kAutoDriveSpeed = .5;
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 1;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Main.java
similarity index 76%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Main.java
index 787aff0..3852d41 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.hatchbotinlined;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Robot.java
new file mode 100644
index 0000000..c0c78ac
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Robot.java
@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
new file mode 100644
index 0000000..9310d39
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.StartEndCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.commands.ComplexAutoCommand;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveDistanceInches;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveSpeed;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems
+ private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+ private final HatchSubsystem m_hatchSubsystem = new HatchSubsystem();
+
+ // The autonomous routines
+
+ // A simple auto routine that drives forward a specified distance, and then stops.
+ private final Command m_simpleAuto =
+ new StartEndCommand(
+ // Start driving forward at the start of the command
+ () -> m_robotDrive.arcadeDrive(kAutoDriveSpeed, 0),
+ // Stop driving at the end of the command
+ () -> m_robotDrive.arcadeDrive(0, 0),
+ // Requires the drive subsystem
+ m_robotDrive
+ )
+ // Reset the encoders before starting
+ .beforeStarting(m_robotDrive::resetEncoders)
+ // End the command when the robot's driven distance exceeds the desired value
+ .withInterrupt(() -> m_robotDrive.getAverageEncoderDistance()
+ >= kAutoDriveDistanceInches);
+
+ // A complex auto routine that drives forward, drops a hatch, and then drives backward.
+ private final Command m_complexAuto = new ComplexAutoCommand(m_robotDrive, m_hatchSubsystem);
+
+ // A chooser for autonomous commands
+ SendableChooser<Command> m_chooser = new SendableChooser<>();
+
+ // The driver's controller
+ XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+
+ // Configure default commands
+ // Set the default drive command to split-stick arcade drive
+ m_robotDrive.setDefaultCommand(
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ new RunCommand(() -> m_robotDrive.arcadeDrive(
+ m_driverController.getY(GenericHID.Hand.kLeft),
+ m_driverController.getX(GenericHID.Hand.kRight)),
+ m_robotDrive)
+ );
+
+ // Add commands to the autonomous command chooser
+ m_chooser.addOption("Simple Auto", m_simpleAuto);
+ m_chooser.addOption("Complex Auto", m_complexAuto);
+
+ // Put the chooser on the dashboard
+ Shuffleboard.getTab("Autonomous").add(m_chooser);
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+ * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ // Grab the hatch when the 'A' button is pressed.
+ new JoystickButton(m_driverController, Button.kA.value)
+ .whenPressed(new InstantCommand(m_hatchSubsystem::grabHatch, m_hatchSubsystem));
+ // Release the hatch when the 'B' button is pressed.
+ new JoystickButton(m_driverController, Button.kB.value)
+ .whenPressed(new InstantCommand(m_hatchSubsystem::releaseHatch, m_hatchSubsystem));
+ // While holding the shoulder button, drive at half speed
+ new JoystickButton(m_driverController, Button.kBumperRight.value)
+ .whenPressed(() -> m_robotDrive.setMaxOutput(.5))
+ .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+ }
+
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ return m_chooser.getSelected();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java
new file mode 100644
index 0000000..f46bcf2
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined.commands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.StartEndCommand;
+
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoBackupDistanceInches;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveDistanceInches;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveSpeed;
+
+/**
+ * A complex auto command that drives forward, releases a hatch, and then drives backward.
+ */
+public class ComplexAutoCommand extends SequentialCommandGroup {
+ /**
+ * Creates a new ComplexAutoCommand.
+ *
+ * @param driveSubsystem The drive subsystem this command will run on
+ * @param hatchSubsystem The hatch subsystem this command will run on
+ */
+ public ComplexAutoCommand(DriveSubsystem driveSubsystem, HatchSubsystem hatchSubsystem) {
+ addCommands(
+ // Drive forward up to the front of the cargo ship
+ new StartEndCommand(
+ // Start driving forward at the start of the command
+ () -> driveSubsystem.arcadeDrive(kAutoDriveSpeed, 0),
+ // Stop driving at the end of the command
+ () -> driveSubsystem.arcadeDrive(0, 0), driveSubsystem
+ )
+ // Reset the encoders before starting
+ .beforeStarting(driveSubsystem::resetEncoders)
+ // End the command when the robot's driven distance exceeds the desired value
+ .withInterrupt(
+ () -> driveSubsystem.getAverageEncoderDistance() >= kAutoDriveDistanceInches),
+
+ // Release the hatch
+ new InstantCommand(hatchSubsystem::releaseHatch, hatchSubsystem),
+
+ // Drive backward the specified distance
+ new StartEndCommand(
+ () -> driveSubsystem.arcadeDrive(-kAutoDriveSpeed, 0),
+ () -> driveSubsystem.arcadeDrive(0, 0),
+ driveSubsystem
+ )
+ .beforeStarting(driveSubsystem::resetEncoders)
+ .withInterrupt(
+ () -> driveSubsystem.getAverageEncoderDistance() <= -kAutoBackupDistanceInches)
+ );
+ }
+
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000..233cb85
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftMotor1Port;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftMotor2Port;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightMotor1Port;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightMotor2Port;
+
+public class DriveSubsystem extends SubsystemBase {
+ // The motors on the left side of the drive.
+ private final SpeedControllerGroup m_leftMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
+ new PWMVictorSPX(kLeftMotor2Port));
+
+ // The motors on the right side of the drive.
+ private final SpeedControllerGroup m_rightMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
+ new PWMVictorSPX(kRightMotor2Port));
+
+ // The robot's drive
+ private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+
+ // The left-side drive encoder
+ private final Encoder m_leftEncoder =
+ new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
+
+ // The right-side drive encoder
+ private final Encoder m_rightEncoder =
+ new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+
+ /**
+ * Creates a new DriveSubsystem.
+ */
+ public DriveSubsystem() {
+ // Sets the distance per pulse for the encoders
+ m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ }
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ public void arcadeDrive(double fwd, double rot) {
+ m_drive.arcadeDrive(fwd, rot);
+ }
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ public void resetEncoders() {
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+ }
+
+ /**
+ * Gets the average distance of the TWO encoders.
+ *
+ * @return the average of the TWO encoder readings
+ */
+ public double getAverageEncoderDistance() {
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+ }
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ public Encoder getLeftEncoder() {
+ return m_leftEncoder;
+ }
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ public Encoder getRightEncoder() {
+ return m_rightEncoder;
+ }
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_drive.setMaxOutput(maxOutput);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java
new file mode 100644
index 0000000..6dce9e1
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
+import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants.kHatchSolenoidModule;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants.kHatchSolenoidPorts;
+
+/**
+ * A hatch mechanism actuated by a single {@link edu.wpi.first.wpilibj.DoubleSolenoid}.
+ */
+public class HatchSubsystem extends SubsystemBase {
+ private final DoubleSolenoid m_hatchSolenoid =
+ new DoubleSolenoid(kHatchSolenoidModule, kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]);
+
+ /**
+ * Grabs the hatch.
+ */
+ public void grabHatch() {
+ m_hatchSolenoid.set(kForward);
+ }
+
+ /**
+ * Releases the hatch.
+ */
+ public void releaseHatch() {
+ m_hatchSolenoid.set(kReverse);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java
new file mode 100644
index 0000000..1e9e060
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kLeftMotor1Port = 0;
+ public static final int kLeftMotor2Port = 1;
+ public static final int kRightMotor1Port = 2;
+ public static final int kRightMotor2Port = 3;
+
+ public static final int[] kLeftEncoderPorts = new int[]{0, 1};
+ public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final boolean kLeftEncoderReversed = false;
+ public static final boolean kRightEncoderReversed = true;
+
+ public static final int kEncoderCPR = 1024;
+ public static final double kWheelDiameterInches = 6;
+ public static final double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
+ }
+
+ public static final class HatchConstants {
+ public static final int kHatchSolenoidModule = 0;
+ public static final int[] kHatchSolenoidPorts = new int[]{0, 1};
+ }
+
+ public static final class AutoConstants {
+ public static final double kAutoDriveDistanceInches = 60;
+ public static final double kAutoBackupDistanceInches = 20;
+ public static final double kAutoDriveSpeed = .5;
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 1;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Main.java
similarity index 75%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Main.java
index 787aff0..f09858c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.hatchbottraditional;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Robot.java
new file mode 100644
index 0000000..8747f35
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Robot.java
@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java
new file mode 100644
index 0000000..f839768
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.ComplexAuto;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.DefaultDrive;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.DriveDistance;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.GrabHatch;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.HalveDriveSpeed;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.ReleaseHatch;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveDistanceInches;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveSpeed;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems
+ private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+ private final HatchSubsystem m_hatchSubsystem = new HatchSubsystem();
+
+ // The autonomous routines
+
+ // A simple auto routine that drives forward a specified distance, and then stops.
+ private final Command m_simpleAuto =
+ new DriveDistance(kAutoDriveDistanceInches, kAutoDriveSpeed, m_robotDrive);
+
+ // A complex auto routine that drives forward, drops a hatch, and then drives backward.
+ private final Command m_complexAuto = new ComplexAuto(m_robotDrive, m_hatchSubsystem);
+
+ // A chooser for autonomous commands
+ SendableChooser<Command> m_chooser = new SendableChooser<>();
+
+ // The driver's controller
+ XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+
+ // Configure default commands
+ // Set the default drive command to split-stick arcade drive
+ m_robotDrive.setDefaultCommand(
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ new DefaultDrive(
+ m_robotDrive,
+ () -> m_driverController.getY(GenericHID.Hand.kLeft),
+ () -> m_driverController.getX(GenericHID.Hand.kRight))
+ );
+
+ // Add commands to the autonomous command chooser
+ m_chooser.addOption("Simple Auto", m_simpleAuto);
+ m_chooser.addOption("Complex Auto", m_complexAuto);
+
+ // Put the chooser on the dashboard
+ Shuffleboard.getTab("Autonomous").add(m_chooser);
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+ * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ // Grab the hatch when the 'A' button is pressed.
+ new JoystickButton(m_driverController, Button.kA.value)
+ .whenPressed(new GrabHatch(m_hatchSubsystem));
+ // Release the hatch when the 'B' button is pressed.
+ new JoystickButton(m_driverController, Button.kB.value)
+ .whenPressed(new ReleaseHatch(m_hatchSubsystem));
+ // While holding the shoulder button, drive at half speed
+ new JoystickButton(m_driverController, Button.kBumperRight.value)
+ .whenHeld(new HalveDriveSpeed(m_robotDrive));
+ }
+
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ return m_chooser.getSelected();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java
new file mode 100644
index 0000000..9614922
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoBackupDistanceInches;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveDistanceInches;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveSpeed;
+
+/**
+ * A complex auto command that drives forward, releases a hatch, and then drives backward.
+ */
+public class ComplexAuto extends SequentialCommandGroup {
+ /**
+ * Creates a new ComplexAuto.
+ *
+ * @param drive The drive subsystem this command will run on
+ * @param hatch The hatch subsystem this command will run on
+ */
+ public ComplexAuto(DriveSubsystem drive, HatchSubsystem hatch) {
+ addCommands(
+ // Drive forward the specified distance
+ new DriveDistance(kAutoDriveDistanceInches, kAutoDriveSpeed, drive),
+
+ // Release the hatch
+ new ReleaseHatch(hatch),
+
+ // Drive backward the specified distance
+ new DriveDistance(kAutoBackupDistanceInches, -kAutoDriveSpeed, drive)
+ );
+ }
+
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DefaultDrive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DefaultDrive.java
new file mode 100644
index 0000000..9399c30
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DefaultDrive.java
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+
+/**
+ * A command to drive the robot with joystick input (passed in as {@link DoubleSupplier}s). Written
+ * explicitly for pedagogical purposes - actual code should inline a command this simple with {@link
+ * edu.wpi.first.wpilibj2.command.RunCommand}.
+ */
+public class DefaultDrive extends CommandBase {
+ private final DriveSubsystem m_drive;
+ private final DoubleSupplier m_forward;
+ private final DoubleSupplier m_rotation;
+
+ /**
+ * Creates a new DefaultDrive.
+ *
+ * @param subsystem The drive subsystem this command wil run on.
+ * @param forward The control input for driving forwards/backwards
+ * @param rotation The control input for turning
+ */
+ public DefaultDrive(DriveSubsystem subsystem, DoubleSupplier forward, DoubleSupplier rotation) {
+ m_drive = subsystem;
+ m_forward = forward;
+ m_rotation = rotation;
+ addRequirements(m_drive);
+ }
+
+ @Override
+ public void execute() {
+ m_drive.arcadeDrive(m_forward.getAsDouble(), m_rotation.getAsDouble());
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java
new file mode 100644
index 0000000..d4abd71
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+
+public class DriveDistance extends CommandBase {
+ private final DriveSubsystem m_drive;
+ private final double m_distance;
+ private final double m_speed;
+
+ /**
+ * Creates a new DriveDistance.
+ *
+ * @param inches The number of inches the robot will drive
+ * @param speed The speed at which the robot will drive
+ * @param drive The drive subsystem on which this command will run
+ */
+ public DriveDistance(double inches, double speed, DriveSubsystem drive) {
+ m_distance = inches;
+ m_speed = speed;
+ m_drive = drive;
+ }
+
+ @Override
+ public void initialize() {
+ m_drive.resetEncoders();
+ m_drive.arcadeDrive(m_speed, 0);
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_drive.arcadeDrive(0, 0);
+ }
+
+ @Override
+ public boolean isFinished() {
+ return Math.abs(m_drive.getAverageEncoderDistance()) >= m_distance;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/GrabHatch.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/GrabHatch.java
new file mode 100644
index 0000000..a30da45
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/GrabHatch.java
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
+
+/**
+ * A simple command that grabs a hatch with the {@link HatchSubsystem}. Written explicitly for
+ * pedagogical purposes. Actual code should inline a command this simple with {@link
+ * edu.wpi.first.wpilibj2.command.InstantCommand}.
+ */
+public class GrabHatch extends CommandBase {
+ // The subsystem the command runs on
+ private final HatchSubsystem m_hatchSubsystem;
+
+ public GrabHatch(HatchSubsystem subsystem) {
+ m_hatchSubsystem = subsystem;
+ addRequirements(m_hatchSubsystem);
+ }
+
+ @Override
+ public void initialize() {
+ m_hatchSubsystem.grabHatch();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return true;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java
new file mode 100644
index 0000000..396d40d
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+
+public class HalveDriveSpeed extends CommandBase {
+ private final DriveSubsystem m_drive;
+
+ public HalveDriveSpeed(DriveSubsystem drive) {
+ m_drive = drive;
+ }
+
+ @Override
+ public void initialize() {
+ m_drive.setMaxOutput(.5);
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_drive.setMaxOutput(1);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ReleaseHatch.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ReleaseHatch.java
new file mode 100644
index 0000000..1e6f0a0
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ReleaseHatch.java
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
+
+/**
+ * A command that releases the hatch.
+ */
+public class ReleaseHatch extends InstantCommand {
+ public ReleaseHatch(HatchSubsystem subsystem) {
+ super(subsystem::releaseHatch, subsystem);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000..6cadc1f
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftMotor1Port;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftMotor2Port;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightMotor1Port;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightMotor2Port;
+
+public class DriveSubsystem extends SubsystemBase {
+ // The motors on the left side of the drive.
+ private final SpeedControllerGroup m_leftMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
+ new PWMVictorSPX(kLeftMotor2Port));
+
+ // The motors on the right side of the drive.
+ private final SpeedControllerGroup m_rightMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
+ new PWMVictorSPX(kRightMotor2Port));
+
+ // The robot's drive
+ private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+
+ // The left-side drive encoder
+ private final Encoder m_leftEncoder =
+ new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
+
+ // The right-side drive encoder
+ private final Encoder m_rightEncoder =
+ new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+
+ /**
+ * Creates a new DriveSubsystem.
+ */
+ public DriveSubsystem() {
+ // Sets the distance per pulse for the encoders
+ m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ }
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ public void arcadeDrive(double fwd, double rot) {
+ m_drive.arcadeDrive(fwd, rot);
+ }
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ public void resetEncoders() {
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+ }
+
+ /**
+ * Gets the average distance of the TWO encoders.
+ *
+ * @return the average of the TWO encoder readings
+ */
+ public double getAverageEncoderDistance() {
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+ }
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ public Encoder getLeftEncoder() {
+ return m_leftEncoder;
+ }
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ public Encoder getRightEncoder() {
+ return m_rightEncoder;
+ }
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_drive.setMaxOutput(maxOutput);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java
new file mode 100644
index 0000000..e93fea4
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
+import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants.kHatchSolenoidModule;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants.kHatchSolenoidPorts;
+
+/**
+ * A hatch mechanism actuated by a single {@link DoubleSolenoid}.
+ */
+public class HatchSubsystem extends SubsystemBase {
+ private final DoubleSolenoid m_hatchSolenoid =
+ new DoubleSolenoid(kHatchSolenoidModule, kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]);
+
+ /**
+ * Grabs the hatch.
+ */
+ public void grabHatch() {
+ m_hatchSolenoid.set(kForward);
+ }
+
+ /**
+ * Releases the hatch.
+ */
+ public void releaseHatch() {
+ m_hatchSolenoid.set(kReverse);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
new file mode 100644
index 0000000..29a6f49
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
@@ -0,0 +1,138 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.mecanumbot;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+
+/**
+ * Represents a mecanum drive style drivetrain.
+ */
+@SuppressWarnings("PMD.TooManyFields")
+public class Drivetrain {
+ public static final double kMaxSpeed = 3.0; // 3 meters per second
+ public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
+
+ private final Spark m_frontLeftMotor = new Spark(1);
+ private final Spark m_frontRightMotor = new Spark(2);
+ private final Spark m_backLeftMotor = new Spark(3);
+ private final Spark m_backRightMotor = new Spark(4);
+
+ private final Encoder m_frontLeftEncoder = new Encoder(0, 1);
+ private final Encoder m_frontRightEncoder = new Encoder(0, 1);
+ private final Encoder m_backLeftEncoder = new Encoder(0, 1);
+ private final Encoder m_backRightEncoder = new Encoder(0, 1);
+
+ private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
+ private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
+ private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
+ private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
+
+ private final PIDController m_frontLeftPIDController = new PIDController(1, 0, 0);
+ private final PIDController m_frontRightPIDController = new PIDController(1, 0, 0);
+ private final PIDController m_backLeftPIDController = new PIDController(1, 0, 0);
+ private final PIDController m_backRightPIDController = new PIDController(1, 0, 0);
+
+ private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+ private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(
+ m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
+ );
+
+ private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics);
+
+ /**
+ * Constructs a MecanumDrive and resets the gyro.
+ */
+ public Drivetrain() {
+ m_gyro.reset();
+ }
+
+ /**
+ * Returns the angle of the robot as a Rotation2d.
+ *
+ * @return The angle of the robot.
+ */
+ public Rotation2d getAngle() {
+ // Negating the angle because WPILib gyros are CW positive.
+ return Rotation2d.fromDegrees(-m_gyro.getAngle());
+ }
+
+ /**
+ * Returns the current state of the drivetrain.
+ *
+ * @return The current state of the drivetrain.
+ */
+ public MecanumDriveWheelSpeeds getCurrentState() {
+ return new MecanumDriveWheelSpeeds(
+ m_frontLeftEncoder.getRate(),
+ m_frontRightEncoder.getRate(),
+ m_backLeftEncoder.getRate(),
+ m_backRightEncoder.getRate()
+ );
+ }
+
+ /**
+ * Set the desired speeds for each wheel.
+ *
+ * @param speeds The desired wheel speeds.
+ */
+ public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
+ final var frontLeftOutput = m_frontLeftPIDController.calculate(
+ m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond
+ );
+ final var frontRightOutput = m_frontRightPIDController.calculate(
+ m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond
+ );
+ final var backLeftOutput = m_backLeftPIDController.calculate(
+ m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond
+ );
+ final var backRightOutput = m_backRightPIDController.calculate(
+ m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond
+ );
+
+ m_frontLeftMotor.set(frontLeftOutput);
+ m_frontRightMotor.set(frontRightOutput);
+ m_backLeftMotor.set(backLeftOutput);
+ m_backRightMotor.set(backRightOutput);
+ }
+
+ /**
+ * Method to drive the robot using joystick info.
+ *
+ * @param xSpeed Speed of the robot in the x direction (forward).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
+ * @param fieldRelative Whether the provided x and y speeds are relative to the field.
+ */
+ @SuppressWarnings("ParameterName")
+ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+ var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(
+ fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
+ xSpeed, ySpeed, rot, getAngle()
+ ) : new ChassisSpeeds(xSpeed, ySpeed, rot)
+ );
+ mecanumDriveWheelSpeeds.normalize(kMaxSpeed);
+ setSpeeds(mecanumDriveWheelSpeeds);
+ }
+
+ /**
+ * Updates the field relative position of the robot.
+ */
+ public void updateOdometry() {
+ m_odometry.update(getAngle(), getCurrentState());
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Main.java
similarity index 89%
rename from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
rename to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Main.java
index 787aff0..27a4b32 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.mecanumbot;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
new file mode 100644
index 0000000..b7540d3
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.mecanumbot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+import static edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxAngularSpeed;
+import static edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed;
+
+public class Robot extends TimedRobot {
+ private final XboxController m_controller = new XboxController(0);
+ private final Drivetrain m_mecanum = new Drivetrain();
+
+ @Override
+ public void autonomousPeriodic() {
+ driveWithJoystick(false);
+ m_mecanum.updateOdometry();
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ driveWithJoystick(true);
+ }
+
+ private void driveWithJoystick(boolean fieldRelative) {
+ // Get the x speed. We are inverting this because Xbox controllers return
+ // negative values when we push forward.
+ final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * kMaxSpeed;
+
+ // Get the y speed or sideways/strafe speed. We are inverting this because
+ // we want a positive value when we pull to the left. Xbox controllers
+ // return positive values when you pull to the right by default.
+ final var ySpeed = -m_controller.getX(GenericHID.Hand.kLeft) * kMaxSpeed;
+
+ // Get the rate of angular rotation. We are inverting this because we want a
+ // positive value when we pull to the left (remember, CCW is positive in
+ // mathematics). Xbox controllers return positive values when you pull to
+ // the right by default.
+ final var rot = -m_controller.getX(GenericHID.Hand.kRight) * kMaxAngularSpeed;
+
+ m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java
index 3e1c397..0f4da55 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -61,7 +61,7 @@
oi = new OI();
// instantiate the command used for the autonomous period
- m_autoChooser = new SendableChooser<Command>();
+ m_autoChooser = new SendableChooser<>();
m_autoChooser.setDefaultOption("Drive and Shoot", new DriveAndShootAutonomous());
m_autoChooser.addOption("Drive Forward", new DriveForward());
SmartDashboard.putData("Auto Mode", m_autoChooser);
@@ -69,7 +69,7 @@
@Override
public void autonomousInit() {
- m_autonomousCommand = (Command) m_autoChooser.getSelected();
+ m_autonomousCommand = m_autoChooser.getSelected();
m_autonomousCommand.start();
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java
index 6bff9ff..853b51a 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -18,7 +18,7 @@
* ball detection, a piston for opening and closing the claw, and a reed switch
* to check if the piston is open.
*/
-public class Collector extends Subsystem {
+public class Collector extends Subsystem implements AutoCloseable {
// Constants for some useful speeds
public static final double kForward = 1;
public static final double kStop = 0;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java
index d27347d..9b165de 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,7 +11,6 @@
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.Victor;
@@ -58,10 +57,6 @@
m_drive.setExpiration(0.1);
m_drive.setMaxOutput(1.0);
- // Configure encoders
- m_rightEncoder.setPIDSourceType(PIDSourceType.kDisplacement);
- m_leftEncoder.setPIDSourceType(PIDSourceType.kDisplacement);
-
if (Robot.isReal()) { // Converts to feet
m_rightEncoder.setDistancePerPulse(0.0785398);
m_leftEncoder.setDistancePerPulse(0.0785398);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java
index e795593..4fc8055 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,7 +11,7 @@
import edu.wpi.first.wpilibj.buttons.Trigger;
/**
- * A custom button that is triggered when two buttons on a Joystick are
+ * A custom button that is triggered when TWO buttons on a Joystick are
* simultaneously pressed.
*/
public class DoubleButton extends Trigger {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java
index 302ce16..cef0916 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,10 +9,10 @@
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.controller.PIDController;
/**
* This is a sample program to demonstrate how to use a soft potentiometer and a
@@ -36,9 +36,7 @@
private static final double kD = -2.0;
private PIDController m_pidController;
- @SuppressWarnings("PMD.SingularField")
private AnalogInput m_potentiometer;
- @SuppressWarnings("PMD.SingularField")
private SpeedController m_elevatorMotor;
private Joystick m_joystick;
@@ -51,17 +49,16 @@
m_elevatorMotor = new PWMVictorSPX(kMotorChannel);
m_joystick = new Joystick(kJoystickChannel);
- m_pidController = new PIDController(kP, kI, kD, m_potentiometer, m_elevatorMotor);
- m_pidController.setInputRange(0, 5);
- }
-
- @Override
- public void teleopInit() {
- m_pidController.enable();
+ m_pidController = new PIDController(kP, kI, kD);
}
@Override
public void teleopPeriodic() {
+ // Run the PID Controller
+ double pidOut
+ = m_pidController.calculate(m_potentiometer.getAverageVoltage());
+ m_elevatorMotor.set(pidOut);
+
// when the button is pressed once, the selected elevator setpoint
// is incremented
boolean currentButtonValue = m_joystick.getTrigger();
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
new file mode 100644
index 0000000..f7aecdd
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.ramsetecommand;
+
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kLeftMotor1Port = 0;
+ public static final int kLeftMotor2Port = 1;
+ public static final int kRightMotor1Port = 2;
+ public static final int kRightMotor2Port = 3;
+
+ public static final int[] kLeftEncoderPorts = new int[]{0, 1};
+ public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final boolean kLeftEncoderReversed = false;
+ public static final boolean kRightEncoderReversed = true;
+
+ public static final double kTrackwidthMeters = .6;
+ public static final DifferentialDriveKinematics kDriveKinematics =
+ new DifferentialDriveKinematics(kTrackwidthMeters);
+
+ public static final int kEncoderCPR = 1024;
+ public static final double kWheelDiameterMeters = .15;
+ public static final double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
+
+ public static final boolean kGyroReversed = true;
+
+ // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+ // These characterization values MUST be determined either experimentally or theoretically
+ // for *your* robot's drive.
+ // The RobotPy Characterization Toolsuite provides a convenient tool for obtaining these
+ // values for your robot.
+ public static final double ksVolts = 1;
+ public static final double kvVoltSecondsPerMeter = .8;
+ public static final double kaVoltSecondsSquaredPerMeter = .15;
+
+ // Example value only - as above, this must be tuned for your drive!
+ public static final double kPDriveVel = .5;
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 1;
+ }
+
+ public static final class AutoConstants {
+ public static final double kMaxSpeedMetersPerSecond = 3;
+ public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+
+ public static final DifferentialDriveKinematicsConstraint kAutoPathConstraints =
+ new DifferentialDriveKinematicsConstraint(DriveConstants.kDriveKinematics,
+ kMaxSpeedMetersPerSecond);
+
+ // Reasonable baseline values for a RAMSETE follower in units of meters and seconds
+ public static final double kRamseteB = 2;
+ public static final double kRamseteZeta = .7;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java
similarity index 76%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java
index 787aff0..4cce085 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.ramsetecommand;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java
new file mode 100644
index 0000000..18dc814
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.ramsetecommand;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
new file mode 100644
index 0000000..f8a3a25
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
@@ -0,0 +1,134 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.ramsetecommand;
+
+import java.util.List;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kRamseteB;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kRamseteZeta;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kDriveKinematics;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kPDriveVel;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kaVoltSecondsSquaredPerMeter;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.ksVolts;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kvVoltSecondsPerMeter;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems
+ private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+
+ // The driver's controller
+ XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+
+ // Configure default commands
+ // Set the default drive command to split-stick arcade drive
+ m_robotDrive.setDefaultCommand(
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ new RunCommand(() -> m_robotDrive
+ .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
+ m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
+
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
+ * {@link JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ // Drive at half speed when the right bumper is held
+ new JoystickButton(m_driverController, Button.kBumperRight.value)
+ .whenPressed(() -> m_robotDrive.setMaxOutput(.5))
+ .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+
+ }
+
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ // Create config for trajectory
+ TrajectoryConfig config =
+ new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared)
+ // Add kinematics to ensure max speed is actually obeyed
+ .setKinematics(kDriveKinematics);
+
+ // An example trajectory to follow. All units in meters.
+ Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
+ // Start at the origin facing the +X direction
+ new Pose2d(0, 0, new Rotation2d(0)),
+ // Pass through these two interior waypoints, making an 's' curve path
+ List.of(
+ new Translation2d(1, 1),
+ new Translation2d(2, -1)
+ ),
+ // End 3 meters straight ahead of where we started, facing forward
+ new Pose2d(3, 0, new Rotation2d(0)),
+ // Pass config
+ config
+ );
+
+ RamseteCommand ramseteCommand = new RamseteCommand(
+ exampleTrajectory,
+ m_robotDrive::getPose,
+ new RamseteController(kRamseteB, kRamseteZeta),
+ ksVolts,
+ kvVoltSecondsPerMeter,
+ kaVoltSecondsSquaredPerMeter,
+ kDriveKinematics,
+ m_robotDrive.getLeftEncoder()::getRate,
+ m_robotDrive.getRightEncoder()::getRate,
+ new PIDController(kPDriveVel, 0, 0),
+ new PIDController(kPDriveVel, 0, 0),
+ // RamseteCommand passes volts to the callback, so we have to rescale here
+ (left, right) -> m_robotDrive.tankDrive(left / 12., right / 12.),
+ m_robotDrive
+ );
+
+ // Run path following command, then stop at the end.
+ return ramseteCommand.andThen(() -> m_robotDrive.tankDrive(0, 0));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000..053fad9
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
@@ -0,0 +1,188 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems;
+
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kDriveKinematics;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kGyroReversed;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftMotor1Port;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftMotor2Port;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightMotor1Port;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightMotor2Port;
+
+public class DriveSubsystem extends SubsystemBase {
+ // The motors on the left side of the drive.
+ private final SpeedControllerGroup m_leftMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
+ new PWMVictorSPX(kLeftMotor2Port));
+
+ // The motors on the right side of the drive.
+ private final SpeedControllerGroup m_rightMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
+ new PWMVictorSPX(kRightMotor2Port));
+
+ // The robot's drive
+ private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+
+ // The left-side drive encoder
+ private final Encoder m_leftEncoder =
+ new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
+
+ // The right-side drive encoder
+ private final Encoder m_rightEncoder =
+ new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+
+ // The gyro sensor
+ private final Gyro m_gyro = new ADXRS450_Gyro();
+
+ // Odometry class for tracking robot pose
+ DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(kDriveKinematics);
+
+ /**
+ * Creates a new DriveSubsystem.
+ */
+ public DriveSubsystem() {
+ // Sets the distance per pulse for the encoders
+ m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ }
+
+ @Override
+ public void periodic() {
+ // Update the odometry in the periodic block
+ m_odometry.update(new Rotation2d(getHeading()),
+ new DifferentialDriveWheelSpeeds(
+ m_leftEncoder.getRate(),
+ m_rightEncoder.getRate()
+ ));
+ }
+
+ /**
+ * Returns the currently-estimated pose of the robot.
+ *
+ * @return The pose.
+ */
+ public Pose2d getPose() {
+ return m_odometry.getPoseMeters();
+ }
+
+ /**
+ * Resets the odometry to the specified pose.
+ *
+ * @param pose The pose to which to set the odometry.
+ */
+ public void resetOdometry(Pose2d pose) {
+ m_odometry.resetPosition(pose);
+ }
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ public void arcadeDrive(double fwd, double rot) {
+ m_drive.arcadeDrive(fwd, rot);
+ }
+
+ /**
+ * Drives the robot using tank controls. Does not square inputs to enable composition with
+ * external controllers.
+ *
+ * @param left the commanded left output
+ * @param right the commanded right output
+ */
+ public void tankDrive(double left, double right) {
+ m_drive.tankDrive(left, right, false);
+ }
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ public void resetEncoders() {
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+ }
+
+ /**
+ * Gets the average distance of the two encoders.
+ *
+ * @return the average of the two encoder readings
+ */
+ public double getAverageEncoderDistance() {
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+ }
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ public Encoder getLeftEncoder() {
+ return m_leftEncoder;
+ }
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ public Encoder getRightEncoder() {
+ return m_rightEncoder;
+ }
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_drive.setMaxOutput(maxOutput);
+ }
+
+ /**
+ * Zeroes the heading of the robot.
+ */
+ public void zeroHeading() {
+ m_gyro.reset();
+ }
+
+ /**
+ * Returns the heading of the robot.
+ *
+ * @return the robot's heading in degrees, from 180 to 180
+ */
+ public double getHeading() {
+ return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1. : 1.);
+ }
+
+ /**
+ * Returns the turn rate of the robot.
+ *
+ * @return The turn rate of the robot, in degrees per second
+ */
+ public double getTurnRate() {
+ return m_gyro.getRate() * (kGyroReversed ? -1. : 1.);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Main.java
similarity index 89%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Main.java
index 787aff0..5cd6a00 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.relay;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Robot.java
new file mode 100644
index 0000000..18da837
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Robot.java
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.relay;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Relay;
+import edu.wpi.first.wpilibj.TimedRobot;
+
+/**
+ * This is a sample program which uses joystick buttons to control a relay. A Relay (generally a
+ * spike) has two outputs, each of which can be at either 0V or 12V and so can be used for actions
+ * such as turning a motor off, full forwards, or full reverse, and is generally used on the
+ * compressor. This program uses two buttons on a joystick and each button corresponds to one
+ * output; pressing the button sets the output to 12V and releasing sets it to 0V.
+ */
+
+public class Robot extends TimedRobot {
+ private final Joystick m_stick = new Joystick(0);
+ private final Relay m_relay = new Relay(0);
+
+ private static final int kRelayForwardButton = 1;
+ private static final int kRelayReverseButton = 2;
+
+ @Override
+ public void teleopPeriodic() {
+ /*
+ * Retrieve the button values. GetRawButton will
+ * return true if the button is pressed and false if not.
+ */
+ boolean forward = m_stick.getRawButton(kRelayForwardButton);
+ boolean reverse = m_stick.getRawButton(kRelayReverseButton);
+
+ /*
+ * Depending on the button values, we want to use one of
+ * kOn, kOff, kForward, or kReverse. kOn sets both outputs to 12V,
+ * kOff sets both to 0V, kForward sets forward to 12V
+ * and reverse to 0V, and kReverse sets reverse to 12V and forward to 0V.
+ */
+ if (forward && reverse) {
+ m_relay.set(Relay.Value.kOn);
+ } else if (forward) {
+ m_relay.set(Relay.Value.kForward);
+ } else if (reverse) {
+ m_relay.set(Relay.Value.kReverse);
+ } else {
+ m_relay.set(Relay.Value.kOff);
+ }
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Constants.java
new file mode 100644
index 0000000..899d074
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Constants.java
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.schedulereventlogging;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ /**
+ * Example of an inner class. One can "import static [...].Constants.OIConstants.*" to gain
+ * access to the constants contained within without having to preface the names with the class,
+ * greatly reducing the amount of text required.
+ */
+ public static final class OIConstants {
+ // Example: the port of the driver's controller
+ public static final int kDriverControllerPort = 1;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Main.java
similarity index 75%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Main.java
index 787aff0..fda3a44 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.schedulereventlogging;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Robot.java
new file mode 100644
index 0000000..bfdf0d7
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Robot.java
@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.schedulereventlogging;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java
new file mode 100644
index 0000000..922b8ff
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java
@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.schedulereventlogging;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.shuffleboard.EventImportance;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.schedulereventlogging.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The driver's controller
+ private final XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+ // A few commands that do nothing, but will demonstrate the scheduler functionality
+ private final CommandBase m_instantCommand1 = new InstantCommand();
+ private final CommandBase m_instantCommand2 = new InstantCommand();
+ private final CommandBase m_waitCommand = new WaitCommand(5);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Set names of commands
+ m_instantCommand1.setName("Instant Command 1");
+ m_instantCommand2.setName("Instant Command 2");
+ m_waitCommand.setName("Wait 5 Seconds Command");
+
+ // Set the scheduler to log Shuffleboard events for command initialize, interrupt, finish
+ CommandScheduler.getInstance().onCommandInitialize(command -> Shuffleboard.addEventMarker(
+ "Command initialized", command.getName(), EventImportance.kNormal));
+ CommandScheduler.getInstance().onCommandInterrupt(command -> Shuffleboard.addEventMarker(
+ "Command interrupted", command.getName(), EventImportance.kNormal));
+ CommandScheduler.getInstance().onCommandFinish(command -> Shuffleboard.addEventMarker(
+ "Command finished", command.getName(), EventImportance.kNormal));
+
+ // Configure the button bindings
+ configureButtonBindings();
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+ * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ // Run instant command 1 when the 'A' button is pressed
+ new JoystickButton(m_driverController, Button.kA.value).whenPressed(m_instantCommand1);
+ // Run instant command 2 when the 'X' button is pressed
+ new JoystickButton(m_driverController, Button.kX.value).whenPressed(m_instantCommand2);
+ // Run instant command 3 when the 'Y' button is held; release early to interrupt
+ new JoystickButton(m_driverController, Button.kY.value).whenHeld(m_waitCommand);
+ }
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ return new InstantCommand();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Constants.java
new file mode 100644
index 0000000..2b73deb
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Constants.java
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.selectcommand;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ /**
+ * Example of an inner class. One can "import static [...].Constants.OIConstants.*" to gain
+ * access to the constants contained within without having to preface the names with the class,
+ * greatly reducing the amount of text required.
+ */
+ public static final class OIConstants {
+ // Example: the port of the driver's controller
+ public static final int kDriverControllerPort = 1;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Main.java
similarity index 76%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Main.java
index 787aff0..a1104ac 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.selectcommand;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Robot.java
new file mode 100644
index 0000000..8d00173
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Robot.java
@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.selectcommand;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java
new file mode 100644
index 0000000..37142e8
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.selectcommand;
+
+import java.util.Map;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
+import edu.wpi.first.wpilibj2.command.SelectCommand;
+
+import static java.util.Map.entry;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The enum used as keys for selecting the command to run.
+ private enum CommandSelector {
+ ONE, TWO, THREE
+ }
+
+ // An example selector method for the selectcommand. Returns the selector that will select
+ // which command to run. Can base this choice on logical conditions evaluated at runtime.
+ private CommandSelector select() {
+ return CommandSelector.ONE;
+ }
+
+ // An example selectcommand. Will select from the three commands based on the value returned
+ // by the selector method at runtime. Note that selectcommand works on Object(), so the
+ // selector does not have to be an enum; it could be any desired type (string, integer,
+ // boolean, double...)
+ private final Command m_exampleSelectCommand =
+ new SelectCommand(
+ // Maps selector values to commands
+ Map.ofEntries(
+ entry(CommandSelector.ONE, new PrintCommand("Command one was selected!")),
+ entry(CommandSelector.TWO, new PrintCommand("Command two was selected!")),
+ entry(CommandSelector.THREE, new PrintCommand("Command three was selected!"))
+ ),
+ this::select
+ );
+
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
+ * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ }
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ return m_exampleSelectCommand;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java
index 7f59e14..adc713d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -30,7 +30,7 @@
@Override
public void robotInit() {
// Add a 'max speed' widget to a tab named 'Configuration', using a number slider
- // The widget will be placed in the second column and row and will be two columns wide
+ // The widget will be placed in the second column and row and will be TWO columns wide
m_maxSpeed = Shuffleboard.getTab("Configuration")
.add("Max Speed", 1)
.withWidget("Number Slider")
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Main.java
similarity index 89%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Main.java
index 787aff0..c62cb75 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.solenoid;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java
new file mode 100644
index 0000000..8fc49bb
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.solenoid;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Solenoid;
+import edu.wpi.first.wpilibj.TimedRobot;
+
+/**
+ * This is a sample program showing the use of the solenoid classes during operator control. Three
+ * buttons from a joystick will be used to control two solenoids: One button to control the position
+ * of a single solenoid and the other two buttons to control a double solenoid. Single solenoids can
+ * either be on or off, such that the air diverted through them goes through either one channel or
+ * the other. Double solenoids have three states: Off, Forward, and Reverse. Forward and Reverse
+ * divert the air through the two channels and correspond to the on and off of a single solenoid,
+ * but a double solenoid can also be "off", where the solenoid will remain in its default power off
+ * state. Additionally, double solenoids take up two channels on your PCM whereas single solenoids
+ * only take a single channel.
+ */
+
+public class Robot extends TimedRobot {
+ private final Joystick m_stick = new Joystick(0);
+
+ // Solenoid corresponds to a single solenoid.
+ private final Solenoid m_solenoid = new Solenoid(0);
+
+ // DoubleSolenoid corresponds to a double solenoid.
+ private final DoubleSolenoid m_doubleSolenoid = new DoubleSolenoid(1, 2);
+
+ private static final int kSolenoidButton = 1;
+ private static final int kDoubleSolenoidForward = 2;
+ private static final int kDoubleSolenoidReverse = 3;
+
+ @Override
+ public void teleopPeriodic() {
+ /*
+ * The output of GetRawButton is true/false depending on whether
+ * the button is pressed; Set takes a boolean for whether
+ * to use the default (false) channel or the other (true).
+ */
+ m_solenoid.set(m_stick.getRawButton(kSolenoidButton));
+
+ /*
+ * In order to set the double solenoid, if just one button
+ * is pressed, set the solenoid to correspond to that button.
+ * If both are pressed, set the solenoid will be set to Forwards.
+ */
+ if (m_stick.getRawButton(kDoubleSolenoidForward)) {
+ m_doubleSolenoid.set(DoubleSolenoid.Value.kForward);
+ } else if (m_stick.getRawButton(kDoubleSolenoidReverse)) {
+ m_doubleSolenoid.set(DoubleSolenoid.Value.kReverse);
+ }
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
new file mode 100644
index 0000000..0ec09f0
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervebot;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry;
+
+/**
+ * Represents a swerve drive style drivetrain.
+ */
+public class Drivetrain {
+ public static final double kMaxSpeed = 3.0; // 3 meters per second
+ public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
+
+ private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
+ private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
+ private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
+ private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
+
+ private final SwerveModule m_frontLeft = new SwerveModule(1, 2);
+ private final SwerveModule m_frontRight = new SwerveModule(3, 4);
+ private final SwerveModule m_backLeft = new SwerveModule(5, 6);
+ private final SwerveModule m_backRight = new SwerveModule(7, 8);
+
+ private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+ private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
+ m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
+ );
+
+ private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics);
+
+ public Drivetrain() {
+ m_gyro.reset();
+ }
+
+ /**
+ * Returns the angle of the robot as a Rotation2d.
+ *
+ * @return The angle of the robot.
+ */
+ public Rotation2d getAngle() {
+ // Negating the angle because WPILib gyros are CW positive.
+ return Rotation2d.fromDegrees(-m_gyro.getAngle());
+ }
+
+ /**
+ * Method to drive the robot using joystick info.
+ *
+ * @param xSpeed Speed of the robot in the x direction (forward).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
+ * @param fieldRelative Whether the provided x and y speeds are relative to the field.
+ */
+ @SuppressWarnings("ParameterName")
+ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+ var swerveModuleStates = m_kinematics.toSwerveModuleStates(
+ fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
+ xSpeed, ySpeed, rot, getAngle())
+ : new ChassisSpeeds(xSpeed, ySpeed, rot)
+ );
+ SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeed);
+ m_frontLeft.setDesiredState(swerveModuleStates[0]);
+ m_frontRight.setDesiredState(swerveModuleStates[1]);
+ m_backLeft.setDesiredState(swerveModuleStates[2]);
+ m_backRight.setDesiredState(swerveModuleStates[3]);
+ }
+
+ /**
+ * Updates the field relative position of the robot.
+ */
+ public void updateOdometry() {
+ m_odometry.update(
+ getAngle(),
+ m_frontLeft.getState(),
+ m_frontRight.getState(),
+ m_backLeft.getState(),
+ m_backRight.getState()
+ );
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Main.java
similarity index 89%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Main.java
index 787aff0..52400e6 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.examples.swervebot;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
new file mode 100644
index 0000000..ff9f28b
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervebot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+import static edu.wpi.first.wpilibj.examples.swervebot.Drivetrain.kMaxAngularSpeed;
+import static edu.wpi.first.wpilibj.examples.swervebot.Drivetrain.kMaxSpeed;
+
+public class Robot extends TimedRobot {
+ private final XboxController m_controller = new XboxController(0);
+ private final Drivetrain m_swerve = new Drivetrain();
+
+ @Override
+ public void autonomousPeriodic() {
+ driveWithJoystick(false);
+ m_swerve.updateOdometry();
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ driveWithJoystick(true);
+ }
+
+ private void driveWithJoystick(boolean fieldRelative) {
+ // Get the x speed. We are inverting this because Xbox controllers return
+ // negative values when we push forward.
+ final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * kMaxSpeed;
+
+ // Get the y speed or sideways/strafe speed. We are inverting this because
+ // we want a positive value when we pull to the left. Xbox controllers
+ // return positive values when you pull to the right by default.
+ final var ySpeed = -m_controller.getX(GenericHID.Hand.kLeft) * kMaxSpeed;
+
+ // Get the rate of angular rotation. We are inverting this because we want a
+ // positive value when we pull to the left (remember, CCW is positive in
+ // mathematics). Xbox controllers return positive values when you pull to
+ // the right by default.
+ final var rot = -m_controller.getX(GenericHID.Hand.kRight) * kMaxAngularSpeed;
+
+ m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
new file mode 100644
index 0000000..58a0c8d
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
@@ -0,0 +1,91 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervebot;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+public class SwerveModule {
+ private static final double kWheelRadius = 0.0508;
+ private static final int kEncoderResolution = 4096;
+
+ private static final double kModuleMaxAngularVelocity = Drivetrain.kMaxAngularSpeed;
+ private static final double kModuleMaxAngularAcceleration
+ = 2 * Math.PI; // radians per second squared
+
+ private final Spark m_driveMotor;
+ private final Spark m_turningMotor;
+
+ private final Encoder m_driveEncoder = new Encoder(0, 1);
+ private final Encoder m_turningEncoder = new Encoder(0, 1);
+
+ private final PIDController m_drivePIDController = new PIDController(1, 0, 0);
+
+ private final ProfiledPIDController m_turningPIDController
+ = new ProfiledPIDController(1, 0, 0,
+ new TrapezoidProfile.Constraints(kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration));
+
+ /**
+ * Constructs a SwerveModule.
+ *
+ * @param driveMotorChannel ID for the drive motor.
+ * @param turningMotorChannel ID for the turning motor.
+ */
+ public SwerveModule(int driveMotorChannel, int turningMotorChannel) {
+ m_driveMotor = new Spark(driveMotorChannel);
+ m_turningMotor = new Spark(turningMotorChannel);
+
+ // Set the distance per pulse for the drive encoder. We can simply use the
+ // distance traveled for one rotation of the wheel divided by the encoder
+ // resolution.
+ m_driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+
+ // Set the distance (in this case, angle) per pulse for the turning encoder.
+ // This is the the angle through an entire rotation (2 * wpi::math::pi)
+ // divided by the encoder resolution.
+ m_turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution);
+
+ // Limit the PID Controller's input range between -pi and pi and set the input
+ // to be continuous.
+ m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
+ }
+
+ /**
+ * Returns the current state of the module.
+ *
+ * @return The current state of the module.
+ */
+ public SwerveModuleState getState() {
+ return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get()));
+ }
+
+ /**
+ * Sets the desired state for the module.
+ *
+ * @param state Desired state with speed and angle.
+ */
+ public void setDesiredState(SwerveModuleState state) {
+ // Calculate the drive output from the drive PID controller.
+ final var driveOutput = m_drivePIDController.calculate(
+ m_driveEncoder.getRate(), state.speedMetersPerSecond);
+
+ // Calculate the turning motor output from the turning PID controller.
+ final var turnOutput = m_turningPIDController.calculate(
+ m_turningEncoder.get(), state.angle.getRadians()
+ );
+
+ // Calculate the turning motor output from the turning PID controller.
+ m_driveMotor.set(driveOutput);
+ m_turningMotor.set(turnOutput);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
index cc80432..225f3ad 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,10 +8,9 @@
package edu.wpi.first.wpilibj.examples.ultrasonicpid;
import edu.wpi.first.wpilibj.AnalogInput;
-import edu.wpi.first.wpilibj.PIDController;
-import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
/**
@@ -22,9 +21,6 @@
// distance in inches the robot wants to stay from an object
private static final double kHoldDistance = 12.0;
- // maximum distance in inches we expect the robot to see
- private static final double kMaxDistance = 24.0;
-
// factor to convert sensor values to a distance in inches
private static final double kValueToInches = 0.125;
@@ -45,27 +41,19 @@
private final DifferentialDrive m_robotDrive
= new DifferentialDrive(new PWMVictorSPX(kLeftMotorPort),
new PWMVictorSPX(kRightMotorPort));
- private final PIDController m_pidController
- = new PIDController(kP, kI, kD, m_ultrasonic, new MyPidOutput());
+ private final PIDController m_pidController = new PIDController(kP, kI, kD);
- /**
- * Drives the robot a set distance from an object using PID control and the
- * ultrasonic sensor.
- */
@Override
public void teleopInit() {
- // Set expected range to 0-24 inches; e.g. at 24 inches from object go
- // full forward, at 0 inches from object go full backward.
- m_pidController.setInputRange(0, kMaxDistance * kValueToInches);
// Set setpoint of the pid controller
m_pidController.setSetpoint(kHoldDistance * kValueToInches);
- m_pidController.enable(); // begin PID control
}
- private class MyPidOutput implements PIDOutput {
- @Override
- public void pidWrite(double output) {
- m_robotDrive.arcadeDrive(output, 0);
- }
+ @Override
+ public void teleopPeriodic() {
+ double pidOutput
+ = m_pidController.calculate(m_ultrasonic.getAverageVoltage());
+
+ m_robotDrive.arcadeDrive(pidOutput, 0);
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Constants.java
new file mode 100644
index 0000000..0184096
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Constants.java
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.templates.commandbased;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java
index 84ddea4..b529f34 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,9 +10,9 @@
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/OI.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/OI.java
deleted file mode 100644
index 990eb2a..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/OI.java
+++ /dev/null
@@ -1,42 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.commandbased;
-
-/**
- * This class is the glue that binds the controls on the physical operator
- * interface to the commands and command groups that allow control of the robot.
- */
-public class OI {
- //// CREATING BUTTONS
- // One type of button is a joystick button which is any button on a
- //// joystick.
- // You create one by telling it which joystick it's on and which button
- // number it is.
- // Joystick stick = new Joystick(port);
- // Button button = new JoystickButton(stick, buttonNumber);
-
- // There are a few additional built in buttons you can use. Additionally,
- // by subclassing Button you can create custom triggers and bind those to
- // commands the same as any other Button.
-
- //// TRIGGERING COMMANDS WITH BUTTONS
- // Once you have a button, it's trivial to bind it to a button in one of
- // three ways:
-
- // Start the command when the button is pressed and let it run the command
- // until it is finished as determined by it's isFinished method.
- // button.whenPressed(new ExampleCommand());
-
- // Run the command while the button is being held down and interrupt it once
- // the button is released.
- // button.whileHeld(new ExampleCommand());
-
- // Start the command when the button is released and let it run the command
- // until it is finished as determined by it's isFinished method.
- // button.whenReleased(new ExampleCommand());
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java
index 7727bff..c0bc795 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,55 +8,49 @@
package edu.wpi.first.wpilibj.templates.commandbased;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.command.Scheduler;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.templates.commandbased.commands.ExampleCommand;
-import edu.wpi.first.wpilibj.templates.commandbased.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the TimedRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the build.gradle file in the
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
- public static ExampleSubsystem m_subsystem = new ExampleSubsystem();
- public static OI m_oi;
+ private Command m_autonomousCommand;
- Command m_autonomousCommand;
- SendableChooser<Command> m_chooser = new SendableChooser<>();
+ private RobotContainer m_robotContainer;
/**
- * This function is run when the robot is first started up and should be
- * used for any initialization code.
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
*/
@Override
public void robotInit() {
- m_oi = new OI();
- m_chooser.setDefaultOption("Default Auto", new ExampleCommand());
- // chooser.addOption("My Auto", new MyAutoCommand());
- SmartDashboard.putData("Auto mode", m_chooser);
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
}
/**
- * This function is called every robot packet, no matter the mode. Use
- * this for items like diagnostics that you want ran during disabled,
- * autonomous, teleoperated and test.
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
}
/**
* This function is called once each time the robot enters Disabled mode.
- * You can use it to reset any subsystem information you want to clear when
- * the robot is disabled.
*/
@Override
public void disabledInit() {
@@ -64,34 +58,18 @@
@Override
public void disabledPeriodic() {
- Scheduler.getInstance().run();
}
/**
- * This autonomous (along with the chooser code above) shows how to select
- * between different autonomous modes using the dashboard. The sendable
- * chooser code works with the Java SmartDashboard. If you prefer the
- * LabVIEW Dashboard, remove all of the chooser code and uncomment the
- * getString code to get the auto name from the text box below the Gyro
- *
- * <p>You can add additional auto modes by adding additional commands to the
- * chooser code above (like the commented example) or additional comparisons
- * to the switch structure below with additional strings & commands.
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
- m_autonomousCommand = m_chooser.getSelected();
-
- /*
- * String autoSelected = SmartDashboard.getString("Auto Selector",
- * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
- * = new MyAutoCommand(); break; case "Default Auto": default:
- * autonomousCommand = new ExampleCommand(); break; }
- */
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
- m_autonomousCommand.start();
+ m_autonomousCommand.schedule();
}
}
@@ -100,7 +78,6 @@
*/
@Override
public void autonomousPeriodic() {
- Scheduler.getInstance().run();
}
@Override
@@ -119,7 +96,12 @@
*/
@Override
public void teleopPeriodic() {
- Scheduler.getInstance().run();
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotContainer.java
new file mode 100644
index 0000000..4592f2e
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotContainer.java
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.templates.commandbased;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.templates.commandbased.commands.ExampleCommand;
+import edu.wpi.first.wpilibj.templates.commandbased.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems and commands are defined here...
+ private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
+
+ private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem);
+
+
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+ * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ }
+
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ // An ExampleCommand will run in autonomous
+ return m_autoCommand;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotMap.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotMap.java
deleted file mode 100644
index ef213c4..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotMap.java
+++ /dev/null
@@ -1,26 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.commandbased;
-
-/**
- * The RobotMap is a mapping from the ports sensors and actuators are wired into
- * to a variable name. This provides flexibility changing wiring, makes checking
- * the wiring easier and significantly reduces the number of magic numbers
- * floating around.
- */
-public class RobotMap {
- // For example to map the left and right motors, you could define the
- // following variables to use with your drivetrain subsystem.
- // public static int leftMotor = 1;
- // public static int rightMotor = 2;
-
- // If you are using multiple modules, make sure to define both the port
- // number and the module. For example you with a rangefinder:
- // public static int rangefinderPort = 1;
- // public static int rangefinderModule = 1;
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java
index 10ceb6e..cc79077 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,42 +7,23 @@
package edu.wpi.first.wpilibj.templates.commandbased.commands;
-import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.templates.commandbased.Robot;
+import edu.wpi.first.wpilibj.templates.commandbased.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
/**
- * An example command. You can replace me with your own command.
+ * An example command that uses an example subsystem.
*/
-public class ExampleCommand extends Command {
- public ExampleCommand() {
- // Use requires() here to declare subsystem dependencies
- requires(Robot.m_subsystem);
- }
+public class ExampleCommand extends CommandBase {
+ @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+ private final ExampleSubsystem m_subsystem;
- // Called just before this Command runs the first time
- @Override
- protected void initialize() {
- }
-
- // Called repeatedly when this Command is scheduled to run
- @Override
- protected void execute() {
- }
-
- // Make this return true when this Command no longer needs to run execute()
- @Override
- protected boolean isFinished() {
- return false;
- }
-
- // Called once after isFinished returns true
- @Override
- protected void end() {
- }
-
- // Called when another command which requires one or more of the same
- // subsystems is scheduled to run
- @Override
- protected void interrupted() {
+ /**
+ * Creates a new ExampleCommand.
+ *
+ * @param subsystem The subsystem used by this command.
+ */
+ public ExampleCommand(ExampleSubsystem subsystem) {
+ m_subsystem = subsystem;
+ addRequirements(subsystem);
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
index a03b73f..b260c77 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,18 +7,21 @@
package edu.wpi.first.wpilibj.templates.commandbased.subsystems;
-import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
-/**
- * An example subsystem. You can replace me with your own Subsystem.
- */
-public class ExampleSubsystem extends Subsystem {
- // Put methods for controlling this subsystem
- // here. Call these from Commands.
+public class ExampleSubsystem extends SubsystemBase {
+ /**
+ * Creates a new ExampleSubsystem.
+ */
+ public ExampleSubsystem() {
+ }
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
@Override
- public void initDefaultCommand() {
- // Set the default command for a subsystem here.
- // setDefaultCommand(new MySpecialCommand());
+ public void periodic() {
+
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java
index 7f38b8a..630be9a 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -18,6 +18,7 @@
* creating this project, you must also update the build.gradle file in the
* project.
*/
+@SuppressWarnings("deprecation")
public class Robot extends IterativeRobot {
private static final String kDefaultAuto = "Default";
private static final String kCustomAuto = "My Auto";
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Main.java
similarity index 88%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Main.java
index 787aff0..65c6268 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.sample;
+package edu.wpi.first.wpilibj.templates.robotbaseskeleton;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
new file mode 100644
index 0000000..615af87
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.templates.robotbaseskeleton;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+
+/**
+ * The VM is configured to automatically run this class. If you change the name
+ * of this class or the package after creating this project, you must also
+ * update the build.gradle file in the project.
+ */
+public class Robot extends RobotBase {
+ public void robotInit() {
+ }
+
+ public void disabled() {
+ }
+
+ public void autonomous() {
+ }
+
+ public void teleop() {
+ }
+
+ public void test() {
+ }
+
+ @SuppressWarnings("PMD.CyclomaticComplexity")
+ @Override
+ public void startCompetition() {
+ robotInit();
+
+ // Tell the DS that the robot is ready to be enabled
+ HAL.observeUserProgramStarting();
+
+ while (!Thread.currentThread().isInterrupted()) {
+ if (isDisabled()) {
+ m_ds.InDisabled(true);
+ disabled();
+ m_ds.InDisabled(false);
+ while (isDisabled()) {
+ m_ds.waitForData();
+ }
+ } else if (isAutonomous()) {
+ m_ds.InAutonomous(true);
+ autonomous();
+ m_ds.InAutonomous(false);
+ while (isAutonomous() && !isDisabled()) {
+ m_ds.waitForData();
+ }
+ } else if (isTest()) {
+ LiveWindow.setEnabled(true);
+ Shuffleboard.enableActuatorWidgets();
+ m_ds.InTest(true);
+ test();
+ m_ds.InTest(false);
+ while (isTest() && isEnabled()) {
+ m_ds.waitForData();
+ }
+ LiveWindow.setEnabled(false);
+ Shuffleboard.disableActuatorWidgets();
+ } else {
+ m_ds.InOperatorControl(true);
+ teleop();
+ m_ds.InOperatorControl(false);
+ while (isOperatorControl() && !isDisabled()) {
+ m_ds.waitForData();
+ }
+ }
+ }
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Robot.java
deleted file mode 100644
index 9a670f0..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Robot.java
+++ /dev/null
@@ -1,155 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.sample;
-
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SampleRobot;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-/**
- * This is a demo program showing the use of the RobotDrive class. The
- * SampleRobot class is the base of a robot application that will automatically
- * call your Autonomous and OperatorControl methods at the right time as
- * controlled by the switches on the driver station or the field controls.
- *
- * <p>The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the SampleRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the build.properties file in the
- * project.
- *
- * <p>WARNING: While it may look like a good choice to use for your code if
- * you're inexperienced, don't. Unless you know what you are doing, complex code
- * will be much more difficult under this system. Use TimedRobot or
- * Command-Based instead if you're new.
- */
-public class Robot extends SampleRobot {
- private static final String kDefaultAuto = "Default";
- private static final String kCustomAuto = "My Auto";
-
- private final DifferentialDrive m_robotDrive
- = new DifferentialDrive(new PWMVictorSPX(0), new PWMVictorSPX(1));
- private final Joystick m_stick = new Joystick(0);
- private final SendableChooser<String> m_chooser = new SendableChooser<>();
-
- public Robot() {
- m_robotDrive.setExpiration(0.1);
- }
-
- @Override
- public void robotInit() {
- m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
- m_chooser.addOption("My Auto", kCustomAuto);
- SmartDashboard.putData("Auto modes", m_chooser);
- }
-
- /**
- * This autonomous (along with the chooser code above) shows how to select
- * between different autonomous modes using the dashboard. The sendable
- * chooser code works with the Java SmartDashboard. If you prefer the
- * LabVIEW Dashboard, remove all of the chooser code and uncomment the
- * getString line to get the auto name from the text box below the Gyro
- *
- * <p>You can add additional auto modes by adding additional comparisons to
- * the if-else structure below with additional strings. If using the
- * SendableChooser make sure to add them to the chooser code above as well.
- *
- * <p>If you wanted to run a similar autonomous mode with an TimedRobot
- * you would write:
- *
- * <blockquote><pre>{@code
- * Timer timer = new Timer();
- *
- * // This function is run once each time the robot enters autonomous mode
- * public void autonomousInit() {
- * timer.reset();
- * timer.start();
- * }
- *
- * // This function is called periodically during autonomous
- * public void autonomousPeriodic() {
- * // Drive for 2 seconds
- * if (timer.get() < 2.0) {
- * myRobot.drive(-0.5, 0.0); // drive forwards half speed
- * } else if (timer.get() < 5.0) {
- * myRobot.drive(-1.0, 0.0); // drive forwards full speed
- * } else {
- * myRobot.drive(0.0, 0.0); // stop robot
- * }
- * }
- * }</pre></blockquote>
- */
- @Override
- public void autonomous() {
- String autoSelected = m_chooser.getSelected();
- // String autoSelected = SmartDashboard.getString("Auto Selector",
- // defaultAuto);
- System.out.println("Auto selected: " + autoSelected);
-
- // MotorSafety improves safety when motors are updated in loops
- // but is disabled here because motor updates are not looped in
- // this autonomous mode.
- m_robotDrive.setSafetyEnabled(false);
-
- switch (autoSelected) {
- case kCustomAuto:
- // Spin at half speed for two seconds
- m_robotDrive.arcadeDrive(0.0, 0.5);
- Timer.delay(2.0);
-
- // Stop robot
- m_robotDrive.arcadeDrive(0.0, 0.0);
- break;
- case kDefaultAuto:
- default:
- // Drive forwards for two seconds
- m_robotDrive.arcadeDrive(-0.5, 0.0);
- Timer.delay(2.0);
-
- // Stop robot
- m_robotDrive.arcadeDrive(0.0, 0.0);
- break;
- }
- }
-
- /**
- * Runs the motors with arcade steering.
- *
- * <p>If you wanted to run a similar teleoperated mode with an TimedRobot
- * you would write:
- *
- * <blockquote><pre>{@code
- * // This function is called periodically during operator control
- * public void teleopPeriodic() {
- * myRobot.arcadeDrive(stick);
- * }
- * }</pre></blockquote>
- */
- @Override
- public void operatorControl() {
- m_robotDrive.setSafetyEnabled(true);
- while (isOperatorControl() && isEnabled()) {
- // Drive arcade style
- m_robotDrive.arcadeDrive(-m_stick.getY(), m_stick.getX());
-
- // The motors will be updated every 5ms
- Timer.delay(0.005);
- }
- }
-
- /**
- * Runs during test mode.
- */
- @Override
- public void test() {
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json
index 430da80..e4dbc1d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json
@@ -30,6 +30,16 @@
"mainclass": "Main"
},
{
+ "name": "RobotBase Skeleton (Advanced)",
+ "description": "Skeleton (stub) code for RobotBase",
+ "tags": [
+ "RobotBase", "Skeleton"
+ ],
+ "foldername": "robotbaseskeleton",
+ "gradlebase": "java",
+ "mainclass": "Main"
+ },
+ {
"name": "Command Robot",
"description": "Command style",
"tags": [
@@ -38,15 +48,5 @@
"foldername": "commandbased",
"gradlebase": "java",
"mainclass": "Main"
- },
- {
- "name": "Sample Robot",
- "description": "Sample style",
- "tags": [
- "Sample"
- ],
- "foldername": "sample",
- "gradlebase": "java",
- "mainclass": "Main"
}
]