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/wpilibcExamples/.styleguide b/wpilibcExamples/.styleguide
index d39dee7..d9369fa 100644
--- a/wpilibcExamples/.styleguide
+++ b/wpilibcExamples/.styleguide
@@ -13,6 +13,7 @@
^cameraserver/
^cscore
^frc/
+ ^frc2/
^networktables/
^ntcore
^opencv2/
diff --git a/wpilibcExamples/build.gradle b/wpilibcExamples/build.gradle
index 3c87203..04a74bd 100644
--- a/wpilibcExamples/build.gradle
+++ b/wpilibcExamples/build.gradle
@@ -30,6 +30,10 @@
templatesMap.put(it, [])
}
+nativeUtils.platformConfigs.named(nativeUtils.wpi.platforms.roborio).configure {
+ cppCompiler.args.remove('-Wno-error=deprecated-declarations')
+ cppCompiler.args.add('-Werror=deprecated-declarations')
+}
ext {
sharedCvConfigs = examplesMap + templatesMap + [commands: []]
@@ -94,9 +98,8 @@
project(':hal').addHalDependency(binary, 'shared')
lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
- if (binary.targetPlatform.architecture.name != 'athena') {
+ if (binary.targetPlatform.name != nativeUtils.wpi.platforms.roborio) {
lib project: ':simulation:halsim_lowfi', library: 'halsim_lowfi', linkage: 'shared'
- lib project: ':simulation:halsim_adx_gyro_accelerometer', library: 'halsim_adx_gyro_accelerometer', linkage: 'shared'
lib project: ':simulation:halsim_print', library: 'halsim_print', linkage: 'shared'
lib project: ':simulation:halsim_ds_nt', library: 'halsim_ds_nt', linkage: 'shared'
}
@@ -140,11 +143,12 @@
binary.tasks.withType(CppCompile) {
if (!(binary.toolChain in VisualCpp)) {
cppCompiler.args "-Wno-error=deprecated-declarations"
+ } else {
+ cppCompiler.args "/wd4996"
}
}
- if (binary.targetPlatform.architecture.name != 'athena') {
+ if (binary.targetPlatform.name != nativeUtils.wpi.platforms.roborio) {
lib project: ':simulation:halsim_lowfi', library: 'halsim_lowfi', linkage: 'shared'
- lib project: ':simulation:halsim_adx_gyro_accelerometer', library: 'halsim_adx_gyro_accelerometer', linkage: 'shared'
lib project: ':simulation:halsim_print', library: 'halsim_print', linkage: 'shared'
lib project: ':simulation:halsim_ds_nt', library: 'halsim_ds_nt', linkage: 'shared'
}
diff --git a/wpilibcExamples/publish.gradle b/wpilibcExamples/publish.gradle
index ec645ae..927d88e 100644
--- a/wpilibcExamples/publish.gradle
+++ b/wpilibcExamples/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/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
new file mode 100644
index 0000000..8dcc1ea
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Drivetrain.h"
+
+frc::DifferentialDriveWheelSpeeds Drivetrain::GetSpeeds() const {
+ return {units::meters_per_second_t(m_leftEncoder.GetRate()),
+ units::meters_per_second_t(m_rightEncoder.GetRate())};
+}
+
+void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
+ const auto leftOutput = m_leftPIDController.Calculate(
+ m_leftEncoder.GetRate(), speeds.left.to<double>());
+ const auto rightOutput = m_rightPIDController.Calculate(
+ m_rightEncoder.GetRate(), speeds.right.to<double>());
+
+ m_leftGroup.Set(leftOutput);
+ m_rightGroup.Set(rightOutput);
+}
+
+void Drivetrain::Drive(units::meters_per_second_t xSpeed,
+ units::radians_per_second_t rot) {
+ SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
+}
+
+void Drivetrain::UpdateOdometry() {
+ m_odometry.Update(GetAngle(), GetSpeeds());
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp
new file mode 100644
index 0000000..b1e858e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+
+#include "Drivetrain.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+ void AutonomousPeriodic() override {
+ TeleopPeriodic();
+ m_drive.UpdateOdometry();
+ }
+
+ void TeleopPeriodic() override {
+ // Get the x speed. We are inverting this because Xbox controllers return
+ // negative values when we push forward.
+ const auto xSpeed =
+ -m_controller.GetY(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed;
+
+ // Get the rate of angular rotation. We are inverting this because we want a
+ // positive value when we pull to the left (remember, CCW is positive in
+ // mathematics). Xbox controllers return positive values when you pull to
+ // the right by default.
+ const auto rot = -m_controller.GetX(frc::GenericHID::kRightHand) *
+ Drivetrain::kMaxAngularSpeed;
+
+ m_drive.Drive(xSpeed, rot);
+ }
+
+ private:
+ frc::XboxController m_controller{0};
+ Drivetrain m_drive;
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
new file mode 100644
index 0000000..bf2ad98
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include <frc/AnalogGyro.h>
+#include <frc/Encoder.h>
+#include <frc/Spark.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/controller/PIDController.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/kinematics/DifferentialDriveOdometry.h>
+#include <wpi/math>
+
+/**
+ * Represents a differential drive style drivetrain.
+ */
+class Drivetrain {
+ 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 * wpi::math::pi * kWheelRadius /
+ kEncoderResolution);
+ m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
+ kEncoderResolution);
+ }
+
+ /**
+ * Get the robot angle as a Rotation2d.
+ */
+ frc::Rotation2d GetAngle() const {
+ // Negating the angle because WPILib Gyros are CW positive.
+ return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle()));
+ }
+
+ static constexpr units::meters_per_second_t kMaxSpeed =
+ 3.0_mps; // 3 meters per second
+ static constexpr units::radians_per_second_t kMaxAngularSpeed{
+ wpi::math::pi}; // 1/2 rotation per second
+
+ frc::DifferentialDriveWheelSpeeds GetSpeeds() const;
+ void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
+ void Drive(units::meters_per_second_t xSpeed,
+ units::radians_per_second_t rot);
+ void UpdateOdometry();
+
+ private:
+ static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
+ static constexpr double kWheelRadius = 0.0508; // meters
+ static constexpr int kEncoderResolution = 4096;
+
+ frc::Spark m_leftMaster{1};
+ frc::Spark m_leftFollower{2};
+ frc::Spark m_rightMaster{3};
+ frc::Spark m_rightFollower{4};
+
+ frc::SpeedControllerGroup m_leftGroup{m_leftMaster, m_leftFollower};
+ frc::SpeedControllerGroup m_rightGroup{m_rightMaster, m_rightFollower};
+
+ frc::Encoder m_leftEncoder{0, 1};
+ frc::Encoder m_rightEncoder{0, 1};
+
+ frc2::PIDController m_leftPIDController{1.0, 0.0, 0.0};
+ frc2::PIDController m_rightPIDController{1.0, 0.0, 0.0};
+
+ frc::AnalogGyro m_gyro{0};
+
+ frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
+ frc::DifferentialDriveOdometry m_odometry{m_kinematics};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
new file mode 100644
index 0000000..95aac42
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <units/units.h>
+
+#include <frc/Encoder.h>
+#include <frc/Joystick.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+
+class Robot : public frc::TimedRobot {
+ public:
+ static constexpr units::second_t kDt = 20_ms;
+
+ Robot() { m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5); }
+
+ void TeleopPeriodic() override {
+ if (m_joystick.GetRawButtonPressed(2)) {
+ m_controller.SetGoal(5_m);
+ } else if (m_joystick.GetRawButtonPressed(3)) {
+ m_controller.SetGoal(0_m);
+ }
+
+ // Run controller and update motor output
+ m_motor.Set(
+ m_controller.Calculate(units::meter_t(m_encoder.GetDistance())));
+ }
+
+ private:
+ frc::Joystick m_joystick{1};
+ frc::Encoder m_encoder{1, 2};
+ frc::PWMVictorSPX m_motor{1};
+
+ // Create a PID controller whose setpoint's change is subject to maximum
+ // velocity and acceleration constraints.
+ frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq};
+ frc::ProfiledPIDController m_controller{1.3, 0.0, 0.7, m_constraints, kDt};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
new file mode 100644
index 0000000..9a15f31
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/Encoder.h>
+#include <frc/Joystick.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/controller/PIDController.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+
+class Robot : public frc::TimedRobot {
+ public:
+ static constexpr units::second_t kDt = 20_ms;
+
+ Robot() { m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5); }
+
+ void TeleopPeriodic() override {
+ if (m_joystick.GetRawButtonPressed(2)) {
+ m_goal = {5_m, 0_mps};
+ } else if (m_joystick.GetRawButtonPressed(3)) {
+ m_goal = {0_m, 0_mps};
+ }
+
+ // Create a motion profile with the given maximum velocity and maximum
+ // acceleration constraints for the next setpoint, the desired goal, and the
+ // current setpoint.
+ frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
+
+ // Retrieve the profiled setpoint for the next timestep. This setpoint moves
+ // toward the goal while obeying the constraints.
+ m_setpoint = profile.Calculate(kDt);
+
+ // Run controller with profiled setpoint and update motor output
+ double output = m_controller.Calculate(m_encoder.GetDistance(),
+ m_setpoint.position.to<double>());
+ m_motor.Set(output);
+ }
+
+ private:
+ frc::Joystick m_joystick{1};
+ frc::Encoder m_encoder{1, 2};
+ frc::PWMVictorSPX m_motor{1};
+ frc2::PIDController m_controller{1.3, 0.0, 0.7, kDt};
+
+ frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq};
+ frc::TrapezoidProfile::State m_goal;
+ frc::TrapezoidProfile::State m_setpoint;
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..729a6c6
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc2/command/button/JoystickButton.h>
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+
+ // Set up default drive command
+ m_drive.SetDefaultCommand(frc2::RunCommand(
+ [this] {
+ m_drive.ArcadeDrive(
+ m_driverController.GetY(frc::GenericHID::kLeftHand),
+ m_driverController.GetX(frc::GenericHID::kRightHand));
+ },
+ {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+
+ // Spin up the shooter when the 'A' button is pressed
+ frc2::JoystickButton(&m_driverController, 1).WhenPressed(&m_spinUpShooter);
+
+ // Turn off the shooter when the 'B' button is pressed
+ frc2::JoystickButton(&m_driverController, 2).WhenPressed(&m_stopShooter);
+
+ // Shoot when the 'X' button is held
+ frc2::JoystickButton(&m_driverController, 3)
+ .WhenPressed(&m_shoot)
+ .WhenReleased(&m_stopFeeder);
+
+ // While holding the shoulder button, drive at half speed
+ frc2::JoystickButton(&m_driverController, 6)
+ .WhenPressed(&m_driveHalfSpeed)
+ .WhenReleased(&m_driveFullSpeed);
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Runs the chosen command in autonomous
+ return &m_autonomousCommand;
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..64be1b8
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+ : m_left1{kLeftMotor1Port},
+ m_left2{kLeftMotor2Port},
+ m_right1{kRightMotor1Port},
+ m_right2{kRightMotor2Port},
+ m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
+ m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+ // Set the distance per pulse for the encoders
+ m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void DriveSubsystem::Periodic() {
+ // Implementation of subsystem periodic method goes here.
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+ m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::ResetEncoders() {
+ m_leftEncoder.Reset();
+ m_rightEncoder.Reset();
+}
+
+double DriveSubsystem::GetAverageEncoderDistance() {
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+}
+
+frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+ m_drive.SetMaxOutput(maxOutput);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
new file mode 100644
index 0000000..d3633b6
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/ShooterSubsystem.h"
+
+#include <frc/controller/PIDController.h>
+
+#include "Constants.h"
+
+using namespace ShooterConstants;
+
+ShooterSubsystem::ShooterSubsystem()
+ : PIDSubsystem(frc2::PIDController(kP, kI, kD)),
+ m_shooterMotor(kShooterMotorPort),
+ m_feederMotor(kFeederMotorPort),
+ m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]) {
+ m_controller.SetTolerance(kShooterToleranceRPS);
+ m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void ShooterSubsystem::UseOutput(double output) {
+ // Use a feedforward of the form kS + kV * velocity
+ m_shooterMotor.Set(output + kSFractional + kVFractional * kShooterTargetRPS);
+}
+
+void ShooterSubsystem::Disable() {
+ // Turn off motor when we disable, since useOutput(0) doesn't stop the motor
+ // due to our feedforward
+ frc2::PIDSubsystem::Disable();
+ m_shooterMotor.Set(0);
+}
+
+bool ShooterSubsystem::AtSetpoint() { return m_controller.AtSetpoint(); }
+
+double ShooterSubsystem::GetMeasurement() { return m_shooterEncoder.GetRate(); }
+
+double ShooterSubsystem::GetSetpoint() { return kShooterTargetRPS; }
+
+void ShooterSubsystem::RunFeeder() { m_feederMotor.Set(kFeederSpeed); }
+
+void ShooterSubsystem::StopFeeder() { m_feederMotor.Set(0); }
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
new file mode 100644
index 0000000..9fbcb27
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants. This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+const int kLeftMotor1Port = 0;
+const int kLeftMotor2Port = 1;
+const int kRightMotor1Port = 2;
+const int kRightMotor2Port = 3;
+
+const int kLeftEncoderPorts[]{0, 1};
+const int kRightEncoderPorts[]{2, 3};
+const bool kLeftEncoderReversed = false;
+const bool kRightEncoderReversed = true;
+
+const int kEncoderCPR = 1024;
+const double kWheelDiameterInches = 6;
+const double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+} // namespace DriveConstants
+
+namespace ShooterConstants {
+const int kEncoderPorts[]{4, 5};
+const bool kEncoderReversed = false;
+const int kEncoderCPR = 1024;
+const double kEncoderDistancePerPulse =
+ // Distance units will be rotations
+ 1. / static_cast<double>(kEncoderCPR);
+
+const int kShooterMotorPort = 4;
+const int kFeederMotorPort = 5;
+
+const double kShooterFreeRPS = 5300;
+const double kShooterTargetRPS = 4000;
+const double kShooterToleranceRPS = 50;
+
+const double kP = 1;
+const double kI = 0;
+const double kD = 0;
+
+// On a real robot the feedforward constants should be empirically determined;
+// these are reasonable guesses.
+const double kSFractional = .05;
+const double kVFractional =
+ // Should have value 1 at free speed...
+ 1. / kShooterFreeRPS;
+
+const double kFeederSpeed = .5;
+} // namespace ShooterConstants
+
+namespace AutoConstants {
+constexpr auto kAutoTimeoutSeconds = 12_s;
+constexpr auto kAutoShootTimeSeconds = 7_s;
+} // namespace AutoConstants
+
+namespace OIConstants {
+const int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h
new file mode 100644
index 0000000..fa173d3
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
+
+#include "RobotContainer.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+ void RobotInit() override;
+ void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
+ void AutonomousInit() override;
+ void AutonomousPeriodic() override;
+ void TeleopInit() override;
+ void TeleopPeriodic() override;
+ void TestPeriodic() override;
+
+ private:
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h
new file mode 100644
index 0000000..2ce1e1c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h
@@ -0,0 +1,101 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/ConditionalCommand.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/WaitCommand.h>
+#include <frc2/command/WaitUntilCommand.h>
+
+#include "Constants.h"
+#include "subsystems/DriveSubsystem.h"
+#include "subsystems/ShooterSubsystem.h"
+
+namespace ac = AutoConstants;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The driver's controller
+ frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+ // The robot's subsystems and commands are defined here...
+
+ // The robot's subsystems
+ DriveSubsystem m_drive;
+ ShooterSubsystem m_shooter;
+
+ // A simple autonomous routine that shoots the loaded frisbees
+ frc2::SequentialCommandGroup m_autonomousCommand =
+ frc2::SequentialCommandGroup{
+ // Start the command by spinning up the shooter...
+ frc2::InstantCommand([this] { m_shooter.Enable(); }, {&m_shooter}),
+ // Wait until the shooter is at speed before feeding the frisbees
+ frc2::WaitUntilCommand([this] { return m_shooter.AtSetpoint(); }),
+ // Start running the feeder
+ frc2::InstantCommand([this] { m_shooter.RunFeeder(); }, {&m_shooter}),
+ // Shoot for the specified time
+ frc2::WaitCommand(ac::kAutoShootTimeSeconds)}
+ // Add a timeout (will end the command if, for instance, the shooter
+ // never gets up to
+ // speed)
+ .WithTimeout(ac::kAutoTimeoutSeconds)
+ // When the command ends, turn off the shooter and the feeder
+ .AndThen([this] {
+ m_shooter.Disable();
+ m_shooter.StopFeeder();
+ });
+
+ // Assorted commands to be bound to buttons
+
+ frc2::InstantCommand m_spinUpShooter{[this] { m_shooter.Enable(); },
+ {&m_shooter}};
+
+ frc2::InstantCommand m_stopShooter{[this] { m_shooter.Disable(); },
+ {&m_shooter}};
+
+ // Shoots if the shooter wheen has reached the target speed
+ frc2::ConditionalCommand m_shoot{
+ // Run the feeder
+ frc2::InstantCommand{[this] { m_shooter.RunFeeder(); }, {&m_shooter}},
+ // Do nothing
+ frc2::InstantCommand(),
+ // Determine which of the above to do based on whether the shooter has
+ // reached the
+ // desired speed
+ [this] { return m_shooter.AtSetpoint(); }};
+
+ frc2::InstantCommand m_stopFeeder{[this] { m_shooter.StopFeeder(); },
+ {&m_shooter}};
+
+ frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+ {}};
+ frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
+ {}};
+
+ // The chooser for the autonomous routines
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..3ed1357
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+ DriveSubsystem();
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic() override;
+
+ // Subsystem methods go here.
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ void ArcadeDrive(double fwd, double rot);
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ void ResetEncoders();
+
+ /**
+ * Gets the average distance of the TWO encoders.
+ *
+ * @return the average of the TWO encoder readings
+ */
+ double GetAverageEncoderDistance();
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ frc::Encoder& GetLeftEncoder();
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ frc::Encoder& GetRightEncoder();
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive
+ * more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ void SetMaxOutput(double maxOutput);
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+
+ // The motor controllers
+ frc::PWMVictorSPX m_left1;
+ frc::PWMVictorSPX m_left2;
+ frc::PWMVictorSPX m_right1;
+ frc::PWMVictorSPX m_right2;
+
+ // The motors on the left side of the drive
+ frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+
+ // The motors on the right side of the drive
+ frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+
+ // The robot's drive
+ frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+
+ // The left-side drive encoder
+ frc::Encoder m_leftEncoder;
+
+ // The right-side drive encoder
+ frc::Encoder m_rightEncoder;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
new file mode 100644
index 0000000..4d30d53
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc2/command/PIDSubsystem.h>
+
+class ShooterSubsystem : public frc2::PIDSubsystem {
+ public:
+ ShooterSubsystem();
+
+ void UseOutput(double output) override;
+
+ double GetSetpoint() override;
+
+ double GetMeasurement() override;
+
+ void Disable() override;
+
+ bool AtSetpoint();
+
+ void RunFeeder();
+
+ void StopFeeder();
+
+ private:
+ frc::PWMVictorSPX m_shooterMotor;
+ frc::PWMVictorSPX m_feederMotor;
+ frc::Encoder m_shooterEncoder;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/OI.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/OI.cpp
deleted file mode 100644
index bd0e922..0000000
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/OI.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "OI.h"
-
-#include <frc/smartdashboard/SmartDashboard.h>
-
-#include "commands/Autonomous.h"
-#include "commands/CloseClaw.h"
-#include "commands/OpenClaw.h"
-#include "commands/Pickup.h"
-#include "commands/Place.h"
-#include "commands/PrepareToPickup.h"
-#include "commands/SetElevatorSetpoint.h"
-
-OI::OI() {
- frc::SmartDashboard::PutData("Open Claw", new OpenClaw());
- frc::SmartDashboard::PutData("Close Claw", new CloseClaw());
-
- // Connect the buttons to commands
- m_dUp.WhenPressed(new SetElevatorSetpoint(0.2));
- m_dDown.WhenPressed(new SetElevatorSetpoint(-0.2));
- m_dRight.WhenPressed(new CloseClaw());
- m_dLeft.WhenPressed(new OpenClaw());
-
- m_r1.WhenPressed(new PrepareToPickup());
- m_r2.WhenPressed(new Pickup());
- m_l1.WhenPressed(new Place());
- m_l2.WhenPressed(new Autonomous());
-}
-
-frc::Joystick& OI::GetJoystick() { return m_joy; }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/Robot.cpp
index ca2f9d6..cd19aeb 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/Robot.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,39 +7,63 @@
#include "Robot.h"
-#include <iostream>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
-DriveTrain Robot::drivetrain;
-Elevator Robot::elevator;
-Wrist Robot::wrist;
-Claw Robot::claw;
-OI Robot::oi;
+void Robot::RobotInit() {}
-void Robot::RobotInit() {
- // Show what command your subsystem is running on the SmartDashboard
- frc::SmartDashboard::PutData(&drivetrain);
- frc::SmartDashboard::PutData(&elevator);
- frc::SmartDashboard::PutData(&wrist);
- frc::SmartDashboard::PutData(&claw);
-}
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
void Robot::AutonomousInit() {
- m_autonomousCommand.Start();
- std::cout << "Starting Auto" << std::endl;
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
}
-void Robot::AutonomousPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+void Robot::AutonomousPeriodic() {}
void Robot::TeleopInit() {
- // This makes sure that the autonomous stops running when teleop starts
- // running. If you want the autonomous to continue until interrupted by
- // another command, remove this line or comment it out.
- m_autonomousCommand.Cancel();
- std::cout << "Starting Teleop" << std::endl;
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
}
-void Robot::TeleopPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+/**
+ * This function is called periodically during test mode.
+ */
void Robot::TestPeriodic() {}
#ifndef RUNNING_FRC_TESTS
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..f79b3fc
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/RobotContainer.cpp
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/button/JoystickButton.h>
+
+#include "commands/CloseClaw.h"
+#include "commands/OpenClaw.h"
+#include "commands/Pickup.h"
+#include "commands/Place.h"
+#include "commands/PrepareToPickup.h"
+#include "commands/SetElevatorSetpoint.h"
+#include "commands/TankDrive.h"
+
+RobotContainer::RobotContainer()
+ : m_autonomousCommand(&m_claw, &m_wrist, &m_elevator, &m_drivetrain) {
+ frc::SmartDashboard::PutData(&m_drivetrain);
+ frc::SmartDashboard::PutData(&m_elevator);
+ frc::SmartDashboard::PutData(&m_wrist);
+ frc::SmartDashboard::PutData(&m_claw);
+
+ m_claw.Log();
+ m_wrist.Log();
+ m_elevator.Log();
+ m_drivetrain.Log();
+
+ m_drivetrain.SetDefaultCommand(TankDrive(
+ [this] { return m_joy.GetY(frc::GenericHID::JoystickHand::kLeftHand); },
+ [this] { return m_joy.GetY(frc::GenericHID::JoystickHand::kRightHand); },
+ &m_drivetrain));
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+ frc2::JoystickButton m_dUp{&m_joy, 5};
+ frc2::JoystickButton m_dRight{&m_joy, 6};
+ frc2::JoystickButton m_dDown{&m_joy, 7};
+ frc2::JoystickButton m_dLeft{&m_joy, 8};
+ frc2::JoystickButton m_l2{&m_joy, 9};
+ frc2::JoystickButton m_r2{&m_joy, 10};
+ frc2::JoystickButton m_l1{&m_joy, 11};
+ frc2::JoystickButton m_r1{&m_joy, 12};
+
+ m_dUp.WhenPressed(SetElevatorSetpoint(0.2, &m_elevator));
+ m_dDown.WhenPressed(SetElevatorSetpoint(-0.2, &m_elevator));
+ m_dRight.WhenPressed(CloseClaw(&m_claw));
+ m_dLeft.WhenPressed(OpenClaw(&m_claw));
+
+ m_r1.WhenPressed(PrepareToPickup(&m_claw, &m_wrist, &m_elevator));
+ m_r2.WhenPressed(Pickup(&m_claw, &m_wrist, &m_elevator));
+ m_l1.WhenPressed(Place(&m_claw, &m_wrist, &m_elevator));
+ m_l2.WhenPressed(Autonomous(&m_claw, &m_wrist, &m_elevator, &m_drivetrain));
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // An example command will be run in autonomous
+ return &m_autonomousCommand;
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp
index 30ce002..5db0cdb 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,6 +7,8 @@
#include "commands/Autonomous.h"
+#include <frc2/command/ParallelCommandGroup.h>
+
#include "commands/CloseClaw.h"
#include "commands/DriveStraight.h"
#include "commands/Pickup.h"
@@ -15,16 +17,19 @@
#include "commands/SetDistanceToBox.h"
#include "commands/SetWristSetpoint.h"
-Autonomous::Autonomous() : frc::CommandGroup("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());
+Autonomous::Autonomous(Claw* claw, Wrist* wrist, Elevator* elevator,
+ DriveTrain* drivetrain) {
+ SetName("Autonomous");
+ AddCommands(
+ // clang-format off
+ PrepareToPickup(claw, wrist, elevator),
+ Pickup(claw, wrist, elevator),
+ SetDistanceToBox(0.10, drivetrain),
+ // DriveStraight(4, drivetrain) // Use encoders if ultrasonic is broken
+ Place(claw, wrist, elevator),
+ SetDistanceToBox(0.6, drivetrain),
+ // DriveStraight(-2, drivetrain) // Use encoders if ultrasonic is broken
+ frc2::ParallelCommandGroup(SetWristSetpoint(-45, wrist),
+ CloseClaw(claw)));
+ // clang-format on
}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp
index e1a2f6e..97a9ccf 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,20 +9,23 @@
#include "Robot.h"
-CloseClaw::CloseClaw() : frc::Command("CloseClaw") { Requires(&Robot::claw); }
+CloseClaw::CloseClaw(Claw* claw) : m_claw(claw) {
+ SetName("CloseClaw");
+ AddRequirements({m_claw});
+}
// Called just before this Command runs the first time
-void CloseClaw::Initialize() { Robot::claw.Close(); }
+void CloseClaw::Initialize() { m_claw->Close(); }
// Make this return true when this Command no longer needs to run execute()
-bool CloseClaw::IsFinished() { return Robot::claw.IsGripping(); }
+bool CloseClaw::IsFinished() { return m_claw->IsGripping(); }
// Called once after isFinished returns true
-void CloseClaw::End() {
+void CloseClaw::End(bool) {
// 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.
#ifndef SIMULATION
- Robot::claw.Stop();
+ m_claw->Stop();
#endif
}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp
index dfd1744..d84951c 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,36 +7,25 @@
#include "commands/DriveStraight.h"
+#include <frc/controller/PIDController.h>
+
#include "Robot.h"
-DriveStraight::DriveStraight(double distance) {
- Requires(&Robot::drivetrain);
- m_pid.SetAbsoluteTolerance(0.01);
- m_pid.SetSetpoint(distance);
+DriveStraight::DriveStraight(double distance, DriveTrain* drivetrain)
+ : frc2::CommandHelper<frc2::PIDCommand, DriveStraight>(
+ frc2::PIDController(4, 0, 0),
+ [this]() { return m_drivetrain->GetDistance(); }, distance,
+ [this](double output) { m_drivetrain->Drive(output, output); },
+ {drivetrain}),
+ m_drivetrain(drivetrain) {
+ m_controller.SetTolerance(0.01);
}
// Called just before this Command runs the first time
void DriveStraight::Initialize() {
// Get everything in a safe starting state.
- Robot::drivetrain.Reset();
- m_pid.Reset();
- m_pid.Enable();
+ m_drivetrain->Reset();
+ frc2::PIDCommand::Initialize();
}
-// Make this return true when this Command no longer needs to run execute()
-bool DriveStraight::IsFinished() { return m_pid.OnTarget(); }
-
-// Called once after isFinished returns true
-void DriveStraight::End() {
- // Stop PID and the wheels
- m_pid.Disable();
- Robot::drivetrain.Drive(0, 0);
-}
-
-double DriveStraight::DriveStraightPIDSource::PIDGet() {
- return Robot::drivetrain.GetDistance();
-}
-
-void DriveStraight::DriveStraightPIDOutput::PIDWrite(double d) {
- Robot::drivetrain.Drive(d, d);
-}
+bool DriveStraight::IsFinished() { return m_controller.AtSetpoint(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp
index 175f3c1..bd1414a 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,16 +9,17 @@
#include "Robot.h"
-OpenClaw::OpenClaw() : frc::Command("OpenClaw") {
- Requires(&Robot::claw);
- SetTimeout(1);
+OpenClaw::OpenClaw(Claw* claw)
+ : frc2::CommandHelper<frc2::WaitCommand, OpenClaw>(1_s), m_claw(claw) {
+ SetName("OpenClaw");
+ AddRequirements({m_claw});
}
// Called just before this Command runs the first time
-void OpenClaw::Initialize() { Robot::claw.Open(); }
-
-// Make this return true when this Command no longer needs to run execute()
-bool OpenClaw::IsFinished() { return IsTimedOut(); }
+void OpenClaw::Initialize() {
+ m_claw->Open();
+ frc2::WaitCommand::Initialize();
+}
// Called once after isFinished returns true
-void OpenClaw::End() { Robot::claw.Stop(); }
+void OpenClaw::End(bool) { m_claw->Stop(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp
index b5d8dd1..996414f 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,12 +7,15 @@
#include "commands/Pickup.h"
+#include <frc2/command/ParallelCommandGroup.h>
+
#include "commands/CloseClaw.h"
#include "commands/SetElevatorSetpoint.h"
#include "commands/SetWristSetpoint.h"
-Pickup::Pickup() : frc::CommandGroup("Pickup") {
- AddSequential(new CloseClaw());
- AddParallel(new SetWristSetpoint(-45));
- AddSequential(new SetElevatorSetpoint(0.25));
+Pickup::Pickup(Claw* claw, Wrist* wrist, Elevator* elevator) {
+ SetName("Pickup");
+ AddCommands(CloseClaw(claw),
+ frc2::ParallelCommandGroup(SetWristSetpoint(-45, wrist),
+ SetElevatorSetpoint(0.25, elevator)));
}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp
index cf31d3c..764ab0e 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,8 +11,11 @@
#include "commands/SetElevatorSetpoint.h"
#include "commands/SetWristSetpoint.h"
-Place::Place() : frc::CommandGroup("Place") {
- AddSequential(new SetElevatorSetpoint(0.25));
- AddSequential(new SetWristSetpoint(0));
- AddSequential(new OpenClaw());
+Place::Place(Claw* claw, Wrist* wrist, Elevator* elevator) {
+ SetName("Place");
+ // clang-format off
+ AddCommands(SetElevatorSetpoint(0.25, elevator),
+ SetWristSetpoint(0, wrist),
+ OpenClaw(claw));
+ // clang-format on
}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp
index af8f0c7..f1399d0 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,12 +7,15 @@
#include "commands/PrepareToPickup.h"
+#include <frc2/command/ParallelCommandGroup.h>
+
#include "commands/OpenClaw.h"
#include "commands/SetElevatorSetpoint.h"
#include "commands/SetWristSetpoint.h"
-PrepareToPickup::PrepareToPickup() : frc::CommandGroup("PrepareToPickup") {
- AddParallel(new OpenClaw());
- AddParallel(new SetWristSetpoint(0));
- AddSequential(new SetElevatorSetpoint(0));
+PrepareToPickup::PrepareToPickup(Claw* claw, Wrist* wrist, Elevator* elevator) {
+ SetName("PrepareToPickup");
+ AddCommands(OpenClaw(claw),
+ frc2::ParallelCommandGroup(SetElevatorSetpoint(0, elevator),
+ SetWristSetpoint(0, wrist)));
}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp
index a0921e8..5da3045 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,38 +7,25 @@
#include "commands/SetDistanceToBox.h"
-#include <frc/PIDController.h>
+#include <frc/controller/PIDController.h>
#include "Robot.h"
-SetDistanceToBox::SetDistanceToBox(double distance) {
- Requires(&Robot::drivetrain);
- m_pid.SetAbsoluteTolerance(0.01);
- m_pid.SetSetpoint(distance);
+SetDistanceToBox::SetDistanceToBox(double distance, DriveTrain* drivetrain)
+ : frc2::CommandHelper<frc2::PIDCommand, SetDistanceToBox>(
+ frc2::PIDController(-2, 0, 0),
+ [this]() { return m_drivetrain->GetDistanceToObstacle(); }, distance,
+ [this](double output) { m_drivetrain->Drive(output, output); },
+ {drivetrain}),
+ m_drivetrain(drivetrain) {
+ m_controller.SetTolerance(0.01);
}
// Called just before this Command runs the first time
void SetDistanceToBox::Initialize() {
// Get everything in a safe starting state.
- Robot::drivetrain.Reset();
- m_pid.Reset();
- m_pid.Enable();
+ m_drivetrain->Reset();
+ frc2::PIDCommand::Initialize();
}
-// Make this return true when this Command no longer needs to run execute()
-bool SetDistanceToBox::IsFinished() { return m_pid.OnTarget(); }
-
-// Called once after isFinished returns true
-void SetDistanceToBox::End() {
- // Stop PID and the wheels
- m_pid.Disable();
- Robot::drivetrain.Drive(0, 0);
-}
-
-double SetDistanceToBox::SetDistanceToBoxPIDSource::PIDGet() {
- return Robot::drivetrain.GetDistanceToObstacle();
-}
-
-void SetDistanceToBox::SetDistanceToBoxPIDOutput::PIDWrite(double d) {
- Robot::drivetrain.Drive(d, d);
-}
+bool SetDistanceToBox::IsFinished() { return m_controller.AtSetpoint(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp
index afe85e5..161c1ee 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,17 +11,19 @@
#include "Robot.h"
-SetElevatorSetpoint::SetElevatorSetpoint(double setpoint)
- : frc::Command("SetElevatorSetpoint") {
- m_setpoint = setpoint;
- Requires(&Robot::elevator);
+SetElevatorSetpoint::SetElevatorSetpoint(double setpoint, Elevator* elevator)
+ : m_setpoint(setpoint), m_elevator(elevator) {
+ SetName("SetElevatorSetpoint");
+ AddRequirements({m_elevator});
}
// Called just before this Command runs the first time
void SetElevatorSetpoint::Initialize() {
- Robot::elevator.SetSetpoint(m_setpoint);
- Robot::elevator.Enable();
+ m_elevator->SetSetpoint(m_setpoint);
+ m_elevator->Enable();
}
// Make this return true when this Command no longer needs to run execute()
-bool SetElevatorSetpoint::IsFinished() { return Robot::elevator.OnTarget(); }
+bool SetElevatorSetpoint::IsFinished() {
+ return m_elevator->GetController().AtSetpoint();
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp
index e9dc2af..50c9d0a 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,17 +9,19 @@
#include "Robot.h"
-SetWristSetpoint::SetWristSetpoint(double setpoint)
- : frc::Command("SetWristSetpoint") {
- m_setpoint = setpoint;
- Requires(&Robot::wrist);
+SetWristSetpoint::SetWristSetpoint(double setpoint, Wrist* wrist)
+ : m_setpoint(setpoint), m_wrist(wrist) {
+ SetName("SetWristSetpoint");
+ AddRequirements({m_wrist});
}
// Called just before this Command runs the first time
void SetWristSetpoint::Initialize() {
- Robot::wrist.SetSetpoint(m_setpoint);
- Robot::wrist.Enable();
+ m_wrist->SetSetpoint(m_setpoint);
+ m_wrist->Enable();
}
// Make this return true when this Command no longer needs to run execute()
-bool SetWristSetpoint::IsFinished() { return Robot::wrist.OnTarget(); }
+bool SetWristSetpoint::IsFinished() {
+ return m_wrist->GetController().AtSetpoint();
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDrive.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDrive.cpp
new file mode 100644
index 0000000..58b1cfe
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDrive.cpp
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/TankDrive.h"
+
+#include "Robot.h"
+
+TankDrive::TankDrive(std::function<double()> left,
+ std::function<double()> right, DriveTrain* drivetrain)
+ : m_left(left), m_right(right), m_drivetrain(drivetrain) {
+ SetName("TankDrive");
+ AddRequirements({m_drivetrain});
+}
+
+// Called repeatedly when this Command is scheduled to run
+void TankDrive::Execute() { m_drivetrain->Drive(m_left(), m_right()); }
+
+// Make this return true when this Command no longer needs to run execute()
+bool TankDrive::IsFinished() { return false; }
+
+// Called once after isFinished returns true
+void TankDrive::End(bool) { m_drivetrain->Drive(0, 0); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDriveWithJoystick.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDriveWithJoystick.cpp
deleted file mode 100644
index a1be9a3..0000000
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDriveWithJoystick.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "commands/TankDriveWithJoystick.h"
-
-#include "Robot.h"
-
-TankDriveWithJoystick::TankDriveWithJoystick()
- : frc::Command("TankDriveWithJoystick") {
- Requires(&Robot::drivetrain);
-}
-
-// Called repeatedly when this Command is scheduled to run
-void TankDriveWithJoystick::Execute() {
- auto& joystick = Robot::oi.GetJoystick();
- Robot::drivetrain.Drive(-joystick.GetY(), -joystick.GetRawAxis(4));
-}
-
-// Make this return true when this Command no longer needs to run execute()
-bool TankDriveWithJoystick::IsFinished() { return false; }
-
-// Called once after isFinished returns true
-void TankDriveWithJoystick::End() { Robot::drivetrain.Drive(0, 0); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp
index 8bad17f..7f2a60f 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,13 +7,12 @@
#include "subsystems/Claw.h"
-Claw::Claw() : frc::Subsystem("Claw") {
+Claw::Claw() {
// Let's show everything on the LiveWindow
- AddChild("Motor", m_motor);
+ SetName("Claw");
+ AddChild("Motor", &m_motor);
}
-void Claw::InitDefaultCommand() {}
-
void Claw::Open() { m_motor.Set(-1); }
void Claw::Close() { m_motor.Set(1); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp
index 4110485..8eec077 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,9 +10,7 @@
#include <frc/Joystick.h>
#include <frc/smartdashboard/SmartDashboard.h>
-#include "commands/TankDriveWithJoystick.h"
-
-DriveTrain::DriveTrain() : frc::Subsystem("DriveTrain") {
+DriveTrain::DriveTrain() {
// Encoders may measure differently in the real world and in
// simulation. In this example the robot moves 0.042 barleycorns
// per tick in the real world, but the simulated encoders
@@ -28,20 +26,16 @@
m_rightEncoder.SetDistancePerPulse(static_cast<double>(4.0 / 12.0 * M_PI) /
360.0);
#endif
-
+ SetName("DriveTrain");
// Let's show everything on the LiveWindow
- // AddChild("Front_Left Motor", m_frontLeft);
- // AddChild("Rear Left Motor", m_rearLeft);
- // AddChild("Front Right Motor", m_frontRight);
- // AddChild("Rear Right Motor", m_rearRight);
- AddChild("Left Encoder", m_leftEncoder);
- AddChild("Right Encoder", m_rightEncoder);
- AddChild("Rangefinder", m_rangefinder);
- AddChild("Gyro", m_gyro);
-}
-
-void DriveTrain::InitDefaultCommand() {
- SetDefaultCommand(new TankDriveWithJoystick());
+ AddChild("Front_Left Motor", &m_frontLeft);
+ AddChild("Rear Left Motor", &m_rearLeft);
+ AddChild("Front Right Motor", &m_frontRight);
+ AddChild("Rear Right Motor", &m_rearRight);
+ AddChild("Left Encoder", &m_leftEncoder);
+ AddChild("Right Encoder", &m_rightEncoder);
+ AddChild("Rangefinder", &m_rangefinder);
+ AddChild("Gyro", &m_gyro);
}
void DriveTrain::Log() {
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
index 5318481..a34c395 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,26 +7,29 @@
#include "subsystems/Elevator.h"
+#include <frc/controller/PIDController.h>
#include <frc/livewindow/LiveWindow.h>
#include <frc/smartdashboard/SmartDashboard.h>
-Elevator::Elevator() : frc::PIDSubsystem("Elevator", kP_real, kI_real, 0.0) {
+Elevator::Elevator()
+ : frc2::PIDSubsystem(frc2::PIDController(kP_real, kI_real, 0)) {
#ifdef SIMULATION // Check for simulation and update PID values
GetPIDController()->SetPID(kP_simulation, kI_simulation, 0, 0);
#endif
- SetAbsoluteTolerance(0.005);
+ m_controller.SetTolerance(0.005);
+ SetName("Elevator");
// Let's show everything on the LiveWindow
- AddChild("Motor", m_motor);
+ AddChild("Motor", &m_motor);
AddChild("Pot", &m_pot);
}
-void Elevator::InitDefaultCommand() {}
+void Elevator::Log() { frc::SmartDashboard::PutData("Wrist Pot", &m_pot); }
-void Elevator::Log() {
- // frc::SmartDashboard::PutData("Wrist Pot", &m_pot);
-}
+double Elevator::GetMeasurement() { return m_pot.Get(); }
-double Elevator::ReturnPIDInput() { return m_pot.Get(); }
+void Elevator::UseOutput(double d) { m_motor.Set(d); }
-void Elevator::UsePIDOutput(double d) { m_motor.Set(d); }
+double Elevator::GetSetpoint() { return m_setpoint; }
+
+void Elevator::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
index 9bab812..7f2dedc 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,25 +7,29 @@
#include "subsystems/Wrist.h"
+#include <frc/controller/PIDController.h>
#include <frc/smartdashboard/SmartDashboard.h>
-Wrist::Wrist() : frc::PIDSubsystem("Wrist", kP_real, 0.0, 0.0) {
+Wrist::Wrist() : frc2::PIDSubsystem(frc2::PIDController(kP_real, 0, 0)) {
#ifdef SIMULATION // Check for simulation and update PID values
GetPIDController()->SetPID(kP_simulation, 0, 0, 0);
#endif
- SetAbsoluteTolerance(2.5);
+ m_controller.SetTolerance(2.5);
+ SetName("Wrist");
// Let's show everything on the LiveWindow
- AddChild("Motor", m_motor);
- AddChild("Pot", m_pot);
+ AddChild("Motor", &m_motor);
+ AddChild("Pot", &m_pot);
}
-void Wrist::InitDefaultCommand() {}
-
void Wrist::Log() {
// frc::SmartDashboard::PutData("Wrist Angle", &m_pot);
}
-double Wrist::ReturnPIDInput() { return m_pot.Get(); }
+double Wrist::GetSetpoint() { return m_setpoint; }
-void Wrist::UsePIDOutput(double d) { m_motor.Set(d); }
+void Wrist::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
+
+double Wrist::GetMeasurement() { return m_pot.Get(); }
+
+void Wrist::UseOutput(double d) { m_motor.Set(d); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/OI.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/OI.h
deleted file mode 100644
index 77e50b9..0000000
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/OI.h
+++ /dev/null
@@ -1,30 +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. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/Joystick.h>
-#include <frc/buttons/JoystickButton.h>
-
-class OI {
- public:
- OI();
- frc::Joystick& GetJoystick();
-
- private:
- frc::Joystick m_joy{0};
-
- // Create some buttons
- frc::JoystickButton m_dUp{&m_joy, 5};
- frc::JoystickButton m_dRight{&m_joy, 6};
- frc::JoystickButton m_dDown{&m_joy, 7};
- frc::JoystickButton m_dLeft{&m_joy, 8};
- frc::JoystickButton m_l2{&m_joy, 9};
- frc::JoystickButton m_r2{&m_joy, 10};
- frc::JoystickButton m_l1{&m_joy, 11};
- frc::JoystickButton m_r1{&m_joy, 12};
-};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.h
index 015bbc0..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,34 +8,26 @@
#pragma once
#include <frc/TimedRobot.h>
-#include <frc/commands/Command.h>
-#include <frc/commands/Scheduler.h>
-#include <frc/livewindow/LiveWindow.h>
-#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/Command.h>
-#include "OI.h"
-#include "commands/Autonomous.h"
-#include "subsystems/Claw.h"
-#include "subsystems/DriveTrain.h"
-#include "subsystems/Elevator.h"
-#include "subsystems/Wrist.h"
+#include "RobotContainer.h"
class Robot : public frc::TimedRobot {
public:
- static DriveTrain drivetrain;
- static Elevator elevator;
- static Wrist wrist;
- static Claw claw;
- static OI oi;
-
- private:
- Autonomous m_autonomousCommand;
- frc::LiveWindow& m_lw = *frc::LiveWindow::GetInstance();
-
void RobotInit() override;
+ void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
void AutonomousInit() override;
void AutonomousPeriodic() override;
void TeleopInit() override;
void TeleopPeriodic() override;
void TestPeriodic() override;
+
+ private:
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/RobotContainer.h
new file mode 100644
index 0000000..50fc7d6
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/RobotContainer.h
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Joystick.h>
+#include <frc2/command/Command.h>
+
+#include "commands/Autonomous.h"
+#include "subsystems/Claw.h"
+#include "subsystems/DriveTrain.h"
+#include "subsystems/Elevator.h"
+#include "subsystems/Wrist.h"
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The robot's subsystems and commands are defined here...
+ frc::Joystick m_joy{0};
+
+ Claw m_claw;
+ Wrist m_wrist;
+ Elevator m_elevator;
+ DriveTrain m_drivetrain;
+
+ Autonomous m_autonomousCommand;
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h
index e90098e..c0d429c 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,12 +7,20 @@
#pragma once
-#include <frc/commands/CommandGroup.h>
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/SequentialCommandGroup.h>
+
+#include "subsystems/Claw.h"
+#include "subsystems/DriveTrain.h"
+#include "subsystems/Elevator.h"
+#include "subsystems/Wrist.h"
/**
* The main autonomous command to pickup and deliver the soda to the box.
*/
-class Autonomous : public frc::CommandGroup {
+class Autonomous
+ : public frc2::CommandHelper<frc2::SequentialCommandGroup, Autonomous> {
public:
- Autonomous();
+ Autonomous(Claw* claw, Wrist* wrist, Elevator* elevator,
+ DriveTrain* drivetrain);
};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h
index bda8a5f..a5e6c0b 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,16 +7,22 @@
#pragma once
-#include <frc/commands/Command.h>
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/Claw.h"
/**
* Opens the claw for one second. Real robots should use sensors, stalling
* motors is BAD!
*/
-class CloseClaw : public frc::Command {
+class CloseClaw : public frc2::CommandHelper<frc2::CommandBase, CloseClaw> {
public:
- CloseClaw();
+ explicit CloseClaw(Claw* claw);
void Initialize() override;
bool IsFinished() override;
- void End() override;
+ void End(bool interrupted) override;
+
+ private:
+ Claw* m_claw;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h
index f03e46c..1955169 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,10 +7,10 @@
#pragma once
-#include <frc/PIDController.h>
-#include <frc/PIDOutput.h>
-#include <frc/PIDSource.h>
-#include <frc/commands/Command.h>
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/PIDCommand.h>
+
+#include "subsystems/DriveTrain.h"
/**
* Drive the given distance straight (negative values go backwards).
@@ -18,27 +18,13 @@
* enabled while this command is running. The input is the averaged
* values of the left and right encoders.
*/
-class DriveStraight : public frc::Command {
+class DriveStraight
+ : public frc2::CommandHelper<frc2::PIDCommand, DriveStraight> {
public:
- explicit DriveStraight(double distance);
+ explicit DriveStraight(double distance, DriveTrain* drivetrain);
void Initialize() override;
bool IsFinished() override;
- void End() override;
-
- class DriveStraightPIDSource : public frc::PIDSource {
- public:
- virtual ~DriveStraightPIDSource() = default;
- double PIDGet() override;
- };
-
- class DriveStraightPIDOutput : public frc::PIDOutput {
- public:
- virtual ~DriveStraightPIDOutput() = default;
- void PIDWrite(double d) override;
- };
private:
- DriveStraightPIDSource m_source;
- DriveStraightPIDOutput m_output;
- frc::PIDController m_pid{4, 0, 0, &m_source, &m_output};
+ DriveTrain* m_drivetrain;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h
index c2a7cfb..486f86b 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,16 +7,21 @@
#pragma once
-#include <frc/commands/Command.h>
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/WaitCommand.h>
+
+#include "subsystems/Claw.h"
/**
* Opens the claw for one second. Real robots should use sensors, stalling
* motors is BAD!
*/
-class OpenClaw : public frc::Command {
+class OpenClaw : public frc2::CommandHelper<frc2::WaitCommand, OpenClaw> {
public:
- OpenClaw();
+ explicit OpenClaw(Claw* claw);
void Initialize() override;
- bool IsFinished() override;
- void End() override;
+ void End(bool interrupted) override;
+
+ private:
+ Claw* m_claw;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h
index ed68990..4d74588 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,13 +7,19 @@
#pragma once
-#include <frc/commands/CommandGroup.h>
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/SequentialCommandGroup.h>
+
+#include "subsystems/Claw.h"
+#include "subsystems/Elevator.h"
+#include "subsystems/Wrist.h"
/**
* Pickup a soda can (if one is between the open claws) and
* get it in a safe state to drive around.
*/
-class Pickup : public frc::CommandGroup {
+class Pickup
+ : public frc2::CommandHelper<frc2::SequentialCommandGroup, Pickup> {
public:
- Pickup();
+ Pickup(Claw* claw, Wrist* wrist, Elevator* elevator);
};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h
index 7695393..85945c3 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,12 +7,17 @@
#pragma once
-#include <frc/commands/CommandGroup.h>
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/SequentialCommandGroup.h>
+
+#include "subsystems/Claw.h"
+#include "subsystems/Elevator.h"
+#include "subsystems/Wrist.h"
/**
* Place a held soda can onto the platform.
*/
-class Place : public frc::CommandGroup {
+class Place : public frc2::CommandHelper<frc2::SequentialCommandGroup, Place> {
public:
- Place();
+ Place(Claw* claw, Wrist* wrist, Elevator* elevator);
};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h
index c58035d..b2aa4d1 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,12 +7,18 @@
#pragma once
-#include <frc/commands/CommandGroup.h>
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/SequentialCommandGroup.h>
+
+#include "subsystems/Claw.h"
+#include "subsystems/Elevator.h"
+#include "subsystems/Wrist.h"
/**
* Make sure the robot is in a state to pickup soda cans.
*/
-class PrepareToPickup : public frc::CommandGroup {
+class PrepareToPickup : public frc2::CommandHelper<frc2::SequentialCommandGroup,
+ PrepareToPickup> {
public:
- PrepareToPickup();
+ PrepareToPickup(Claw* claw, Wrist* wrist, Elevator* elevator);
};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h
index 884a09b..b5b7b13 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,10 +7,10 @@
#pragma once
-#include <frc/PIDController.h>
-#include <frc/PIDOutput.h>
-#include <frc/PIDSource.h>
-#include <frc/commands/Command.h>
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/PIDCommand.h>
+
+#include "subsystems/DriveTrain.h"
/**
* Drive until the robot is the given distance away from the box. Uses a local
@@ -18,27 +18,13 @@
* command is running. The input is the averaged values of the left and right
* encoders.
*/
-class SetDistanceToBox : public frc::Command {
+class SetDistanceToBox
+ : public frc2::CommandHelper<frc2::PIDCommand, SetDistanceToBox> {
public:
- explicit SetDistanceToBox(double distance);
+ explicit SetDistanceToBox(double distance, DriveTrain* drivetrain);
void Initialize() override;
bool IsFinished() override;
- void End() override;
-
- class SetDistanceToBoxPIDSource : public frc::PIDSource {
- public:
- virtual ~SetDistanceToBoxPIDSource() = default;
- double PIDGet() override;
- };
-
- class SetDistanceToBoxPIDOutput : public frc::PIDOutput {
- public:
- virtual ~SetDistanceToBoxPIDOutput() = default;
- void PIDWrite(double d) override;
- };
private:
- SetDistanceToBoxPIDSource m_source;
- SetDistanceToBoxPIDOutput m_output;
- frc::PIDController m_pid{-2, 0, 0, &m_source, &m_output};
+ DriveTrain* m_drivetrain;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h
index 388ac0e..c3bcf6f 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,7 +7,10 @@
#pragma once
-#include <frc/commands/Command.h>
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/Elevator.h"
/**
* Move the elevator to a given location. This command finishes when it is
@@ -16,12 +19,14 @@
* Other
* commands using the elevator should make sure they disable PID!
*/
-class SetElevatorSetpoint : public frc::Command {
+class SetElevatorSetpoint
+ : public frc2::CommandHelper<frc2::CommandBase, SetElevatorSetpoint> {
public:
- explicit SetElevatorSetpoint(double setpoint);
+ explicit SetElevatorSetpoint(double setpoint, Elevator* elevator);
void Initialize() override;
bool IsFinished() override;
private:
double m_setpoint;
+ Elevator* m_elevator;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h
index effb173..96c01fa 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,19 +7,24 @@
#pragma once
-#include <frc/commands/Command.h>
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/Wrist.h"
/**
* 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!
*/
-class SetWristSetpoint : public frc::Command {
+class SetWristSetpoint
+ : public frc2::CommandHelper<frc2::CommandBase, SetWristSetpoint> {
public:
- explicit SetWristSetpoint(double setpoint);
+ explicit SetWristSetpoint(double setpoint, Wrist* wrist);
void Initialize() override;
bool IsFinished() override;
private:
double m_setpoint;
+ Wrist* m_wrist;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDrive.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDrive.h
new file mode 100644
index 0000000..bdfd592
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDrive.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/DriveTrain.h"
+
+/**
+ * Have the robot drive tank style using the PS3 Joystick until interrupted.
+ */
+class TankDrive : public frc2::CommandHelper<frc2::CommandBase, TankDrive> {
+ public:
+ TankDrive(std::function<double()> left, std::function<double()> right,
+ DriveTrain* drivetrain);
+ void Execute() override;
+ bool IsFinished() override;
+ void End(bool interrupted) override;
+
+ private:
+ std::function<double()> m_left;
+ std::function<double()> m_right;
+ DriveTrain* m_drivetrain;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDriveWithJoystick.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDriveWithJoystick.h
deleted file mode 100644
index 9337025..0000000
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDriveWithJoystick.h
+++ /dev/null
@@ -1,21 +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. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/Command.h>
-
-/**
- * Have the robot drive tank style using the PS3 Joystick until interrupted.
- */
-class TankDriveWithJoystick : public frc::Command {
- public:
- TankDriveWithJoystick();
- void Execute() override;
- bool IsFinished() override;
- void End() override;
-};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h
index 96a436a..063ca43 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,19 +9,17 @@
#include <frc/DigitalInput.h>
#include <frc/PWMVictorSPX.h>
-#include <frc/commands/Subsystem.h>
+#include <frc2/command/SubsystemBase.h>
/**
* 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.
*/
-class Claw : public frc::Subsystem {
+class Claw : public frc2::SubsystemBase {
public:
Claw();
- void InitDefaultCommand() override;
-
/**
* Set the claw motor to move in the open direction.
*/
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h
index dce4d9c..a80429d 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -12,8 +12,8 @@
#include <frc/Encoder.h>
#include <frc/PWMVictorSPX.h>
#include <frc/SpeedControllerGroup.h>
-#include <frc/commands/Subsystem.h>
#include <frc/drive/DifferentialDrive.h>
+#include <frc2/command/SubsystemBase.h>
namespace frc {
class Joystick;
@@ -24,17 +24,11 @@
* the robots chassis. These include four drive motors, a left and right encoder
* and a gyro.
*/
-class DriveTrain : public frc::Subsystem {
+class DriveTrain : public frc2::SubsystemBase {
public:
DriveTrain();
/**
- * When no other command is running let the operator drive around
- * using the PS3 joystick.
- */
- void InitDefaultCommand() override;
-
- /**
* The log method puts interesting information to the SmartDashboard.
*/
void Log();
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
index 53f6057..a2c1e87 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,7 +9,7 @@
#include <frc/AnalogPotentiometer.h>
#include <frc/PWMVictorSPX.h>
-#include <frc/commands/PIDSubsystem.h>
+#include <frc2/command/PIDSubsystem.h>
/**
* The elevator subsystem uses PID to go to a given height. Unfortunately, in
@@ -17,12 +17,10 @@
* state PID values for simulation are different than in the real world do to
* minor differences.
*/
-class Elevator : public frc::PIDSubsystem {
+class Elevator : public frc2::PIDSubsystem {
public:
Elevator();
- void InitDefaultCommand() override;
-
/**
* The log method puts interesting information to the SmartDashboard.
*/
@@ -32,17 +30,25 @@
* Use the potentiometer as the PID sensor. This method is automatically
* called by the subsystem.
*/
- double ReturnPIDInput() override;
+ double GetMeasurement() override;
/**
* Use the motor as the PID output. This method is automatically called
* by
* the subsystem.
*/
- void UsePIDOutput(double d) override;
+ void UseOutput(double d) override;
+
+ double GetSetpoint() override;
+
+ /**
+ * Sets the setpoint for the subsystem.
+ */
+ void SetSetpoint(double setpoint);
private:
frc::PWMVictorSPX m_motor{5};
+ double m_setpoint = 0;
// Conversion value of potentiometer varies between the real world and
// simulation
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
index 26fc74b..e3ecc69 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,18 +9,16 @@
#include <frc/AnalogPotentiometer.h>
#include <frc/PWMVictorSPX.h>
-#include <frc/commands/PIDSubsystem.h>
+#include <frc2/command/PIDSubsystem.h>
/**
* The wrist subsystem is like the elevator, but with a rotational joint instead
* of a linear joint.
*/
-class Wrist : public frc::PIDSubsystem {
+class Wrist : public frc2::PIDSubsystem {
public:
Wrist();
- void InitDefaultCommand() override;
-
/**
* The log method puts interesting information to the SmartDashboard.
*/
@@ -30,17 +28,25 @@
* Use the potentiometer as the PID sensor. This method is automatically
* called by the subsystem.
*/
- double ReturnPIDInput() override;
+ double GetMeasurement() override;
/**
* Use the motor as the PID output. This method is automatically called
* by
* the subsystem.
*/
- void UsePIDOutput(double d) override;
+ void UseOutput(double d) override;
+
+ double GetSetpoint() override;
+
+ /**
+ * Sets the setpoint for the subsystem.
+ */
+ void SetSetpoint(double setpoint);
private:
frc::PWMVictorSPX m_motor{6};
+ double m_setpoint = 0;
// Conversion value of potentiometer varies between the real world and
// simulation
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..aa1117a
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc2/command/button/JoystickButton.h>
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+
+ // Set up default drive command
+ m_drive.SetDefaultCommand(frc2::RunCommand(
+ [this] {
+ m_drive.ArcadeDrive(
+ m_driverController.GetY(frc::GenericHID::kLeftHand),
+ m_driverController.GetX(frc::GenericHID::kRightHand));
+ },
+ {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+
+ // Stabilize robot to drive straight with gyro when left bumper is held
+ frc2::JoystickButton(&m_driverController, 5).WhenHeld(&m_stabilizeDriving);
+
+ // Turn to 90 degrees when the 'X' button is pressed
+ frc2::JoystickButton(&m_driverController, 3).WhenPressed(&m_turnTo90);
+
+ // While holding the shoulder button, drive at half speed
+ frc2::JoystickButton(&m_driverController, 6)
+ .WhenPressed(&m_driveHalfSpeed)
+ .WhenReleased(&m_driveFullSpeed);
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // no auto
+ return nullptr;
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
new file mode 100644
index 0000000..cadb7f9
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/TurnToAngle.h"
+
+#include <frc/controller/PIDController.h>
+
+using namespace DriveConstants;
+
+TurnToAngle::TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive)
+ : CommandHelper(frc2::PIDController(kTurnP, kTurnI, kTurnD),
+ // Close loop on heading
+ [drive] { return drive->GetHeading(); },
+ // Set reference to target
+ targetAngleDegrees,
+ // Pipe output to turn robot
+ [drive](double output) { drive->ArcadeDrive(0, output); },
+ // Require the drive
+ {drive}) {
+ // Set the controller to be continuous (because it is an angle controller)
+ m_controller.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
+ m_controller.SetTolerance(kTurnToleranceDeg, kTurnRateToleranceDegPerS);
+
+ AddRequirements({drive});
+}
+
+bool TurnToAngle::IsFinished() { return m_controller.AtSetpoint(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..7e5ef96
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+ : m_left1{kLeftMotor1Port},
+ m_left2{kLeftMotor2Port},
+ m_right1{kRightMotor1Port},
+ m_right2{kRightMotor2Port},
+ m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
+ m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+ // Set the distance per pulse for the encoders
+ m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void DriveSubsystem::Periodic() {
+ // Implementation of subsystem periodic method goes here.
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+ m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::ResetEncoders() {
+ m_leftEncoder.Reset();
+ m_rightEncoder.Reset();
+}
+
+double DriveSubsystem::GetAverageEncoderDistance() {
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+}
+
+frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+ m_drive.SetMaxOutput(maxOutput);
+}
+
+double DriveSubsystem::GetHeading() {
+ return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+}
+
+double DriveSubsystem::GetTurnRate() {
+ return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
new file mode 100644
index 0000000..0349c71
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants. This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+const int kLeftMotor1Port = 0;
+const int kLeftMotor2Port = 1;
+const int kRightMotor1Port = 2;
+const int kRightMotor2Port = 3;
+
+const int kLeftEncoderPorts[]{0, 1};
+const int kRightEncoderPorts[]{2, 3};
+const bool kLeftEncoderReversed = false;
+const bool kRightEncoderReversed = true;
+
+const int kEncoderCPR = 1024;
+const double kWheelDiameterInches = 6;
+const double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+
+const bool kGyroReversed = true;
+
+const double kStabilizationP = 1;
+const double kStabilizationI = .5;
+const double kStabilizationD = 0;
+
+const double kTurnP = 1;
+const double kTurnI = 0;
+const double kTurnD = 0;
+
+const double kTurnToleranceDeg = 5;
+const double kTurnRateToleranceDegPerS = 10; // degrees per second
+} // namespace DriveConstants
+
+namespace AutoConstants {
+const double kAutoDriveDistanceInches = 60;
+const double kAutoBackupDistanceInches = 20;
+const double kAutoDriveSpeed = .5;
+} // namespace AutoConstants
+
+namespace OIConstants {
+const int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h
new file mode 100644
index 0000000..fa173d3
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
+
+#include "RobotContainer.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+ void RobotInit() override;
+ void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
+ void AutonomousInit() override;
+ void AutonomousPeriodic() override;
+ void TeleopInit() override;
+ void TeleopPeriodic() override;
+ void TestPeriodic() override;
+
+ private:
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
new file mode 100644
index 0000000..f480536
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/controller/PIDController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/PIDCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+
+#include "Constants.h"
+#include "commands/TurnToAngle.h"
+#include "subsystems/DriveSubsystem.h"
+
+namespace ac = AutoConstants;
+namespace dc = DriveConstants;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The driver's controller
+ frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+ // The robot's subsystems and commands are defined here...
+
+ // The robot's subsystems
+ DriveSubsystem m_drive;
+
+ // Assorted commands to be bound to buttons
+
+ // Turn to 90 degrees, with a 5 second timeout
+ frc2::ParallelRaceGroup m_turnTo90 =
+ TurnToAngle{90, &m_drive}.WithTimeout(5_s);
+
+ // Stabilize the robot while driving
+ frc2::PIDCommand m_stabilizeDriving{
+ frc2::PIDController{dc::kStabilizationP, dc::kStabilizationI,
+ dc::kStabilizationD},
+ // Close the loop on the turn rate
+ [this] { return m_drive.GetTurnRate(); },
+ // Setpoint is 0
+ 0,
+ // Pipe the output to the turning controls
+ [this](double output) {
+ m_drive.ArcadeDrive(
+ m_driverController.GetY(frc::GenericHID::JoystickHand::kLeftHand),
+ output);
+ },
+ // Require the robot drive
+ {&m_drive}};
+
+ frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+ {}};
+ frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
+ {}};
+
+ // The chooser for the autonomous routines
+ frc::SendableChooser<frc2::Command*> m_chooser;
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h
new file mode 100644
index 0000000..c7e875e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/PIDCommand.h>
+
+#include "subsystems/DriveSubsystem.h"
+
+/**
+ * A command that will turn the robot to the specified angle.
+ */
+class TurnToAngle : public frc2::CommandHelper<frc2::PIDCommand, TurnToAngle> {
+ public:
+ /**
+ * Turns to robot to the specified angle.
+ *
+ * @param targetAngleDegrees The angle to turn to
+ * @param drive The drive subsystem to use
+ */
+ TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive);
+
+ bool IsFinished() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..cada816
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/ADXRS450_Gyro.h>
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+ DriveSubsystem();
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic() override;
+
+ // Subsystem methods go here.
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ void ArcadeDrive(double fwd, double rot);
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ void ResetEncoders();
+
+ /**
+ * Gets the average distance of the TWO encoders.
+ *
+ * @return the average of the TWO encoder readings
+ */
+ double GetAverageEncoderDistance();
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ frc::Encoder& GetLeftEncoder();
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ frc::Encoder& GetRightEncoder();
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive
+ * more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ void SetMaxOutput(double maxOutput);
+
+ /**
+ * Returns the heading of the robot.
+ *
+ * @return the robot's heading in degrees, from 180 to 180
+ */
+ double GetHeading();
+
+ /**
+ * Returns the turn rate of the robot.
+ *
+ * @return The turn rate of the robot, in degrees per second
+ */
+ double GetTurnRate();
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+
+ // The motor controllers
+ frc::PWMVictorSPX m_left1;
+ frc::PWMVictorSPX m_left2;
+ frc::PWMVictorSPX m_right1;
+ frc::PWMVictorSPX m_right2;
+
+ // The motors on the left side of the drive
+ frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+
+ // The motors on the right side of the drive
+ frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+
+ // The robot's drive
+ frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+
+ // The left-side drive encoder
+ frc::Encoder m_leftEncoder;
+
+ // The right-side drive encoder
+ frc::Encoder m_rightEncoder;
+
+ // The gyro sensor
+ frc::ADXRS450_Gyro m_gyro;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..f8719c4
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc2/command/button/JoystickButton.h>
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // 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
+ frc::Shuffleboard::GetTab("Autonomous").Add(m_chooser);
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+
+ // Set up default drive command
+ m_drive.SetDefaultCommand(frc2::RunCommand(
+ [this] {
+ m_drive.ArcadeDrive(
+ m_driverController.GetY(frc::GenericHID::kLeftHand),
+ m_driverController.GetX(frc::GenericHID::kRightHand));
+ },
+ {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+
+ // Grab the hatch when the 'A' button is pressed.
+ frc2::JoystickButton(&m_driverController, 1).WhenPressed(&m_grabHatch);
+ // Release the hatch when the 'B' button is pressed.
+ frc2::JoystickButton(&m_driverController, 2).WhenPressed(&m_releaseHatch);
+ // While holding the shoulder button, drive at half speed
+ frc2::JoystickButton(&m_driverController, 6)
+ .WhenPressed(&m_driveHalfSpeed)
+ .WhenReleased(&m_driveFullSpeed);
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Runs the chosen command in autonomous
+ return m_chooser.GetSelected();
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp
new file mode 100644
index 0000000..c5f928b
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/ComplexAuto.h"
+
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/StartEndCommand.h>
+
+using namespace AutoConstants;
+
+ComplexAuto::ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch) {
+ AddCommands(
+ // Drive forward the specified distance
+ frc2::StartEndCommand([drive] { drive->ArcadeDrive(kAutoDriveSpeed, 0); },
+ [drive] { drive->ArcadeDrive(0, 0); }, {drive})
+ .BeforeStarting([drive] { drive->ResetEncoders(); })
+ .WithInterrupt([drive] {
+ return drive->GetAverageEncoderDistance() >=
+ kAutoDriveDistanceInches;
+ }),
+ // Release the hatch
+ frc2::InstantCommand([hatch] { hatch->ReleaseHatch(); }, {hatch}),
+ // Drive backward the specified distance
+ frc2::StartEndCommand(
+ [drive] { drive->ArcadeDrive(-kAutoDriveSpeed, 0); },
+ [drive] { drive->ArcadeDrive(0, 0); }, {drive})
+ .BeforeStarting([drive] { drive->ResetEncoders(); })
+ .WithInterrupt([drive] {
+ return drive->GetAverageEncoderDistance() <=
+ kAutoBackupDistanceInches;
+ }));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..64be1b8
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+ : m_left1{kLeftMotor1Port},
+ m_left2{kLeftMotor2Port},
+ m_right1{kRightMotor1Port},
+ m_right2{kRightMotor2Port},
+ m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
+ m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+ // Set the distance per pulse for the encoders
+ m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void DriveSubsystem::Periodic() {
+ // Implementation of subsystem periodic method goes here.
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+ m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::ResetEncoders() {
+ m_leftEncoder.Reset();
+ m_rightEncoder.Reset();
+}
+
+double DriveSubsystem::GetAverageEncoderDistance() {
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+}
+
+frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+ m_drive.SetMaxOutput(maxOutput);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp
new file mode 100644
index 0000000..ea7b796
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/HatchSubsystem.h"
+
+using namespace HatchConstants;
+
+HatchSubsystem::HatchSubsystem()
+ : m_hatchSolenoid{kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {}
+
+void HatchSubsystem::GrabHatch() {
+ m_hatchSolenoid.Set(frc::DoubleSolenoid::kForward);
+}
+
+void HatchSubsystem::ReleaseHatch() {
+ m_hatchSolenoid.Set(frc::DoubleSolenoid::kReverse);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
new file mode 100644
index 0000000..b09572e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants. This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+const int kLeftMotor1Port = 0;
+const int kLeftMotor2Port = 1;
+const int kRightMotor1Port = 2;
+const int kRightMotor2Port = 3;
+
+const int kLeftEncoderPorts[]{0, 1};
+const int kRightEncoderPorts[]{2, 3};
+const bool kLeftEncoderReversed = false;
+const bool kRightEncoderReversed = true;
+
+const int kEncoderCPR = 1024;
+const double kWheelDiameterInches = 6;
+const double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+} // namespace DriveConstants
+
+namespace HatchConstants {
+const int kHatchSolenoidModule = 0;
+const int kHatchSolenoidPorts[]{0, 1};
+} // namespace HatchConstants
+
+namespace AutoConstants {
+const double kAutoDriveDistanceInches = 60;
+const double kAutoBackupDistanceInches = 20;
+const double kAutoDriveSpeed = .5;
+} // namespace AutoConstants
+
+namespace OIConstants {
+const int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Robot.h
new file mode 100644
index 0000000..fa173d3
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Robot.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
+
+#include "RobotContainer.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+ void RobotInit() override;
+ void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
+ void AutonomousInit() override;
+ void AutonomousPeriodic() override;
+ void TeleopInit() override;
+ void TeleopPeriodic() override;
+ void TestPeriodic() override;
+
+ private:
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h
new file mode 100644
index 0000000..b7c57a3
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/StartEndCommand.h>
+
+#include "Constants.h"
+#include "commands/ComplexAuto.h"
+#include "subsystems/DriveSubsystem.h"
+#include "subsystems/HatchSubsystem.h"
+
+namespace ac = AutoConstants;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The driver's controller
+ frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+ // The robot's subsystems and commands are defined here...
+
+ // The robot's subsystems
+ DriveSubsystem m_drive;
+ HatchSubsystem m_hatch;
+
+ // The autonomous routines
+ frc2::ParallelRaceGroup m_simpleAuto =
+ frc2::StartEndCommand(
+ [this] { m_drive.ArcadeDrive(ac::kAutoDriveSpeed, 0); },
+ [this] { m_drive.ArcadeDrive(0, 0); }, {&m_drive})
+ .BeforeStarting([this] { m_drive.ResetEncoders(); })
+ .WithInterrupt([this] {
+ return m_drive.GetAverageEncoderDistance() >=
+ ac::kAutoDriveDistanceInches;
+ });
+ ComplexAuto m_complexAuto{&m_drive, &m_hatch};
+
+ // Assorted commands to be bound to buttons
+
+ frc2::InstantCommand m_grabHatch{[this] { m_hatch.GrabHatch(); }, {&m_hatch}};
+ frc2::InstantCommand m_releaseHatch{[this] { m_hatch.ReleaseHatch(); },
+ {&m_hatch}};
+ frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+ {}};
+ frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
+ {}};
+
+ // The chooser for the autonomous routines
+ frc::SendableChooser<frc2::Command*> m_chooser;
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h
new file mode 100644
index 0000000..b767f3b
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/SequentialCommandGroup.h>
+
+#include "Constants.h"
+#include "subsystems/DriveSubsystem.h"
+#include "subsystems/HatchSubsystem.h"
+
+/**
+ * A complex auto command that drives forward, releases a hatch, and then drives
+ * backward.
+ */
+class ComplexAuto
+ : public frc2::CommandHelper<frc2::SequentialCommandGroup, ComplexAuto> {
+ public:
+ /**
+ * Creates a new ComplexAuto.
+ *
+ * @param drive The drive subsystem this command will run on
+ * @param hatch The hatch subsystem this command will run on
+ */
+ ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch);
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..3ed1357
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+ DriveSubsystem();
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic() override;
+
+ // Subsystem methods go here.
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ void ArcadeDrive(double fwd, double rot);
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ void ResetEncoders();
+
+ /**
+ * Gets the average distance of the TWO encoders.
+ *
+ * @return the average of the TWO encoder readings
+ */
+ double GetAverageEncoderDistance();
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ frc::Encoder& GetLeftEncoder();
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ frc::Encoder& GetRightEncoder();
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive
+ * more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ void SetMaxOutput(double maxOutput);
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+
+ // The motor controllers
+ frc::PWMVictorSPX m_left1;
+ frc::PWMVictorSPX m_left2;
+ frc::PWMVictorSPX m_right1;
+ frc::PWMVictorSPX m_right2;
+
+ // The motors on the left side of the drive
+ frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+
+ // The motors on the right side of the drive
+ frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+
+ // The robot's drive
+ frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+
+ // The left-side drive encoder
+ frc::Encoder m_leftEncoder;
+
+ // The right-side drive encoder
+ frc::Encoder m_rightEncoder;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h
new file mode 100644
index 0000000..681aea8
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/DoubleSolenoid.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class HatchSubsystem : public frc2::SubsystemBase {
+ public:
+ HatchSubsystem();
+
+ // Subsystem methods go here.
+
+ /**
+ * Grabs the hatch.
+ */
+ void GrabHatch();
+
+ /**
+ * Releases the hatch.
+ */
+ void ReleaseHatch();
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+ frc::DoubleSolenoid m_hatchSolenoid;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..9eb9178
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc2/command/button/JoystickButton.h>
+
+#include "commands/DefaultDrive.h"
+#include "commands/GrabHatch.h"
+#include "commands/HalveDriveSpeed.h"
+#include "commands/ReleaseHatch.h"
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // 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
+ frc::Shuffleboard::GetTab("Autonomous").Add(m_chooser);
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+
+ // Set up default drive command
+ m_drive.SetDefaultCommand(DefaultDrive(
+ &m_drive,
+ [this] { return m_driverController.GetY(frc::GenericHID::kLeftHand); },
+ [this] { return m_driverController.GetX(frc::GenericHID::kRightHand); }));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+
+ // NOTE: Using `new` here will leak these commands if they are ever no longer
+ // needed. This is usually a non-issue as button-bindings tend to be permanent
+ // - however, if you wish to avoid this, the commands should be
+ // stack-allocated and declared as members of RobotContainer.
+
+ // Grab the hatch when the 'A' button is pressed.
+ frc2::JoystickButton(&m_driverController, 1)
+ .WhenPressed(new GrabHatch(&m_hatch));
+ // Release the hatch when the 'B' button is pressed.
+ frc2::JoystickButton(&m_driverController, 2)
+ .WhenPressed(new ReleaseHatch(&m_hatch));
+ // While holding the shoulder button, drive at half speed
+ frc2::JoystickButton(&m_driverController, 6)
+ .WhenHeld(new HalveDriveSpeed(&m_drive));
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Runs the chosen command in autonomous
+ return m_chooser.GetSelected();
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ComplexAuto.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ComplexAuto.cpp
new file mode 100644
index 0000000..cb41de6
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ComplexAuto.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/ComplexAuto.h"
+
+using namespace AutoConstants;
+
+ComplexAuto::ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch) {
+ AddCommands(
+ // Drive forward the specified distance
+ DriveDistance(kAutoDriveDistanceInches, kAutoDriveSpeed, drive),
+ // Release the hatch
+ ReleaseHatch(hatch),
+ // Drive backward the specified distance
+ DriveDistance(kAutoBackupDistanceInches, -kAutoDriveSpeed, drive));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp
new file mode 100644
index 0000000..3bdee6e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/DefaultDrive.h"
+
+DefaultDrive::DefaultDrive(DriveSubsystem* subsystem,
+ std::function<double()> forward,
+ std::function<double()> rotation)
+ : m_drive{subsystem}, m_forward{forward}, m_rotation{rotation} {
+ AddRequirements({subsystem});
+}
+
+void DefaultDrive::Execute() {
+ m_drive->ArcadeDrive(m_forward(), m_rotation());
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp
new file mode 100644
index 0000000..6c7ef40
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/DriveDistance.h"
+
+#include <cmath>
+
+DriveDistance::DriveDistance(double inches, double speed,
+ DriveSubsystem* subsystem)
+ : m_drive(subsystem), m_distance(inches), m_speed(speed) {
+ AddRequirements({subsystem});
+}
+
+void DriveDistance::Initialize() {
+ m_drive->ResetEncoders();
+ m_drive->ArcadeDrive(m_speed, 0);
+}
+
+void DriveDistance::End(bool interrupted) { m_drive->ArcadeDrive(0, 0); }
+
+bool DriveDistance::IsFinished() {
+ return std::abs(m_drive->GetAverageEncoderDistance()) >= m_distance;
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/GrabHatch.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/GrabHatch.cpp
new file mode 100644
index 0000000..f665761
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/GrabHatch.cpp
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/GrabHatch.h"
+
+GrabHatch::GrabHatch(HatchSubsystem* subsystem) : m_hatch(subsystem) {
+ AddRequirements({subsystem});
+}
+
+void GrabHatch::Initialize() { m_hatch->GrabHatch(); }
+
+bool GrabHatch::IsFinished() { return true; }
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp
new file mode 100644
index 0000000..839bb87
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/HalveDriveSpeed.h"
+
+HalveDriveSpeed::HalveDriveSpeed(DriveSubsystem* subsystem)
+ : m_drive(subsystem) {}
+
+void HalveDriveSpeed::Initialize() { m_drive->SetMaxOutput(.5); }
+
+void HalveDriveSpeed::End(bool interrupted) { m_drive->SetMaxOutput(1); }
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp
new file mode 100644
index 0000000..e8fbd61
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/ReleaseHatch.h"
+
+ReleaseHatch::ReleaseHatch(HatchSubsystem* subsystem) : m_hatch(subsystem) {
+ AddRequirements({subsystem});
+}
+
+void ReleaseHatch::Initialize() { m_hatch->ReleaseHatch(); }
+
+bool ReleaseHatch::IsFinished() { return true; }
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..64be1b8
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+ : m_left1{kLeftMotor1Port},
+ m_left2{kLeftMotor2Port},
+ m_right1{kRightMotor1Port},
+ m_right2{kRightMotor2Port},
+ m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
+ m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+ // Set the distance per pulse for the encoders
+ m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void DriveSubsystem::Periodic() {
+ // Implementation of subsystem periodic method goes here.
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+ m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::ResetEncoders() {
+ m_leftEncoder.Reset();
+ m_rightEncoder.Reset();
+}
+
+double DriveSubsystem::GetAverageEncoderDistance() {
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+}
+
+frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+ m_drive.SetMaxOutput(maxOutput);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp
new file mode 100644
index 0000000..ea7b796
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/HatchSubsystem.h"
+
+using namespace HatchConstants;
+
+HatchSubsystem::HatchSubsystem()
+ : m_hatchSolenoid{kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {}
+
+void HatchSubsystem::GrabHatch() {
+ m_hatchSolenoid.Set(frc::DoubleSolenoid::kForward);
+}
+
+void HatchSubsystem::ReleaseHatch() {
+ m_hatchSolenoid.Set(frc::DoubleSolenoid::kReverse);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
new file mode 100644
index 0000000..b09572e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants. This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+const int kLeftMotor1Port = 0;
+const int kLeftMotor2Port = 1;
+const int kRightMotor1Port = 2;
+const int kRightMotor2Port = 3;
+
+const int kLeftEncoderPorts[]{0, 1};
+const int kRightEncoderPorts[]{2, 3};
+const bool kLeftEncoderReversed = false;
+const bool kRightEncoderReversed = true;
+
+const int kEncoderCPR = 1024;
+const double kWheelDiameterInches = 6;
+const double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+} // namespace DriveConstants
+
+namespace HatchConstants {
+const int kHatchSolenoidModule = 0;
+const int kHatchSolenoidPorts[]{0, 1};
+} // namespace HatchConstants
+
+namespace AutoConstants {
+const double kAutoDriveDistanceInches = 60;
+const double kAutoBackupDistanceInches = 20;
+const double kAutoDriveSpeed = .5;
+} // namespace AutoConstants
+
+namespace OIConstants {
+const int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Robot.h
new file mode 100644
index 0000000..fa173d3
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Robot.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
+
+#include "RobotContainer.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+ void RobotInit() override;
+ void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
+ void AutonomousInit() override;
+ void AutonomousPeriodic() override;
+ void TeleopInit() override;
+ void TeleopPeriodic() override;
+ void TestPeriodic() override;
+
+ private:
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/RobotContainer.h
new file mode 100644
index 0000000..881d2f5
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/RobotContainer.h
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+
+#include "Constants.h"
+#include "commands/ComplexAuto.h"
+#include "commands/DefaultDrive.h"
+#include "commands/DriveDistance.h"
+#include "subsystems/DriveSubsystem.h"
+#include "subsystems/HatchSubsystem.h"
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The robot's subsystems and commands are defined here...
+
+ // The robot's subsystems
+ DriveSubsystem m_drive;
+ HatchSubsystem m_hatch;
+
+ // The autonomous routines
+ DriveDistance m_simpleAuto{AutoConstants::kAutoDriveDistanceInches,
+ AutoConstants::kAutoDriveSpeed, &m_drive};
+ ComplexAuto m_complexAuto{&m_drive, &m_hatch};
+
+ // The chooser for the autonomous routines
+ frc::SendableChooser<frc2::Command*> m_chooser;
+
+ // The driver's controller
+ frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ComplexAuto.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ComplexAuto.h
new file mode 100644
index 0000000..88a2460
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ComplexAuto.h
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/SequentialCommandGroup.h>
+
+#include "Constants.h"
+#include "commands/DriveDistance.h"
+#include "commands/ReleaseHatch.h"
+
+/**
+ * A complex auto command that drives forward, releases a hatch, and then drives
+ * backward.
+ */
+class ComplexAuto
+ : public frc2::CommandHelper<frc2::SequentialCommandGroup, ComplexAuto> {
+ public:
+ /**
+ * Creates a new ComplexAuto.
+ *
+ * @param drive The drive subsystem this command will run on
+ * @param hatch The hatch subsystem this command will run on
+ */
+ ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch);
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DefaultDrive.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DefaultDrive.h
new file mode 100644
index 0000000..d42d133
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DefaultDrive.h
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/DriveSubsystem.h"
+
+/**
+ * A command to drive the robot with joystick input passed in through lambdas.
+ * Written explicitly for pedagogical purposes - actual code should inline a
+ * command this simple with RunCommand.
+ *
+ * @see RunCommand
+ */
+class DefaultDrive
+ : public frc2::CommandHelper<frc2::CommandBase, DefaultDrive> {
+ public:
+ /**
+ * 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
+ */
+ DefaultDrive(DriveSubsystem* subsystem, std::function<double()> forward,
+ std::function<double()> rotation);
+
+ void Execute() override;
+
+ private:
+ DriveSubsystem* m_drive;
+ std::function<double()> m_forward;
+ std::function<double()> m_rotation;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h
new file mode 100644
index 0000000..6f350a9
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/DriveSubsystem.h"
+
+class DriveDistance
+ : public frc2::CommandHelper<frc2::CommandBase, DriveDistance> {
+ public:
+ /**
+ * 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
+ */
+ DriveDistance(double inches, double speed, DriveSubsystem* subsystem);
+
+ void Initialize() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ private:
+ DriveSubsystem* m_drive;
+ double m_distance;
+ double m_speed;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/GrabHatch.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/GrabHatch.h
new file mode 100644
index 0000000..0ab0c13
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/GrabHatch.h
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/HatchSubsystem.h"
+
+/**
+ * A simple command that grabs a hatch with the HatchSubsystem. Written
+ * explicitly for pedagogical purposes. Actual code should inline a command
+ * this simple with InstantCommand.
+ *
+ * @see InstantCommand
+ */
+class GrabHatch : public frc2::CommandHelper<frc2::CommandBase, GrabHatch> {
+ public:
+ explicit GrabHatch(HatchSubsystem* subsystem);
+
+ void Initialize() override;
+
+ bool IsFinished() override;
+
+ private:
+ HatchSubsystem* m_hatch;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/HalveDriveSpeed.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/HalveDriveSpeed.h
new file mode 100644
index 0000000..0b5d7c7
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/HalveDriveSpeed.h
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/DriveSubsystem.h"
+
+class HalveDriveSpeed
+ : public frc2::CommandHelper<frc2::CommandBase, HalveDriveSpeed> {
+ public:
+ explicit HalveDriveSpeed(DriveSubsystem* subsystem);
+
+ void Initialize() override;
+
+ void End(bool interrupted) override;
+
+ private:
+ DriveSubsystem* m_drive;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ReleaseHatch.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ReleaseHatch.h
new file mode 100644
index 0000000..b98866f
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ReleaseHatch.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/HatchSubsystem.h"
+
+/**
+ * A simple command that releases a hatch with the HatchSubsystem. Written
+ * explicitly for pedagogical purposes. Actual code should inline a command
+ * this simple with InstantCommand.
+ *
+ * @see InstantCommand
+ */
+class ReleaseHatch
+ : public frc2::CommandHelper<frc2::CommandBase, ReleaseHatch> {
+ public:
+ explicit ReleaseHatch(HatchSubsystem* subsystem);
+
+ void Initialize() override;
+
+ bool IsFinished() override;
+
+ private:
+ HatchSubsystem* m_hatch;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..3ed1357
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+ DriveSubsystem();
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic() override;
+
+ // Subsystem methods go here.
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ void ArcadeDrive(double fwd, double rot);
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ void ResetEncoders();
+
+ /**
+ * Gets the average distance of the TWO encoders.
+ *
+ * @return the average of the TWO encoder readings
+ */
+ double GetAverageEncoderDistance();
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ frc::Encoder& GetLeftEncoder();
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ frc::Encoder& GetRightEncoder();
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive
+ * more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ void SetMaxOutput(double maxOutput);
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+
+ // The motor controllers
+ frc::PWMVictorSPX m_left1;
+ frc::PWMVictorSPX m_left2;
+ frc::PWMVictorSPX m_right1;
+ frc::PWMVictorSPX m_right2;
+
+ // The motors on the left side of the drive
+ frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+
+ // The motors on the right side of the drive
+ frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+
+ // The robot's drive
+ frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+
+ // The left-side drive encoder
+ frc::Encoder m_leftEncoder;
+
+ // The right-side drive encoder
+ frc::Encoder m_rightEncoder;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h
new file mode 100644
index 0000000..681aea8
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/DoubleSolenoid.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class HatchSubsystem : public frc2::SubsystemBase {
+ public:
+ HatchSubsystem();
+
+ // Subsystem methods go here.
+
+ /**
+ * Grabs the hatch.
+ */
+ void GrabHatch();
+
+ /**
+ * Releases the hatch.
+ */
+ void ReleaseHatch();
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+ frc::DoubleSolenoid m_hatchSolenoid;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/IntermediateVision/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/IntermediateVision/cpp/Robot.cpp
index ae2ac83..01a9d04 100644
--- a/wpilibcExamples/src/main/cpp/examples/IntermediateVision/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/IntermediateVision/cpp/Robot.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -61,8 +61,8 @@
#endif
void RobotInit() override {
- // We need to run our vision program in a separate thread. If not, our robot
- // program will not run.
+ // We need to run our vision program in a separate thread. If not, our robot
+ // program will not run.
#if defined(__linux__)
std::thread visionThread(VisionThread);
visionThread.detach();
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
new file mode 100644
index 0000000..5e4fc8c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Drivetrain.h"
+
+frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
+ return {units::meters_per_second_t(m_frontLeftEncoder.GetRate()),
+ units::meters_per_second_t(m_frontRightEncoder.GetRate()),
+ units::meters_per_second_t(m_backLeftEncoder.GetRate()),
+ units::meters_per_second_t(m_backRightEncoder.GetRate())};
+}
+
+void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
+ const auto frontLeftOutput = m_frontLeftPIDController.Calculate(
+ m_frontLeftEncoder.GetRate(), wheelSpeeds.frontLeft.to<double>());
+ const auto frontRightOutput = m_frontRightPIDController.Calculate(
+ m_frontRightEncoder.GetRate(), wheelSpeeds.frontRight.to<double>());
+ const auto backLeftOutput = m_backLeftPIDController.Calculate(
+ m_backLeftEncoder.GetRate(), wheelSpeeds.rearLeft.to<double>());
+ const auto backRightOutput = m_backRightPIDController.Calculate(
+ m_backRightEncoder.GetRate(), wheelSpeeds.rearRight.to<double>());
+
+ m_frontLeftMotor.Set(frontLeftOutput);
+ m_frontRightMotor.Set(frontRightOutput);
+ m_backLeftMotor.Set(backLeftOutput);
+ m_backRightMotor.Set(backRightOutput);
+}
+
+void Drivetrain::Drive(units::meters_per_second_t xSpeed,
+ units::meters_per_second_t ySpeed,
+ units::radians_per_second_t rot, bool fieldRelative) {
+ auto wheelSpeeds = m_kinematics.ToWheelSpeeds(
+ fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+ xSpeed, ySpeed, rot, GetAngle())
+ : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
+ wheelSpeeds.Normalize(kMaxSpeed);
+ SetSpeeds(wheelSpeeds);
+}
+
+void Drivetrain::UpdateOdometry() {
+ m_odometry.Update(GetAngle(), GetCurrentState());
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp
new file mode 100644
index 0000000..5ff14d1
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+
+#include "Drivetrain.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+ void AutonomousPeriodic() override {
+ DriveWithJoystick(false);
+ m_mecanum.UpdateOdometry();
+ }
+
+ void TeleopPeriodic() override { DriveWithJoystick(true); }
+
+ private:
+ frc::XboxController m_controller{0};
+ Drivetrain m_mecanum;
+
+ void DriveWithJoystick(bool fieldRelative) {
+ // Get the x speed. We are inverting this because Xbox controllers return
+ // negative values when we push forward.
+ const auto xSpeed =
+ -m_controller.GetY(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed;
+
+ // Get the y speed or sideways/strafe speed. We are inverting this because
+ // we want a positive value when we pull to the left. Xbox controllers
+ // return positive values when you pull to the right by default.
+ const auto ySpeed =
+ -m_controller.GetX(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed;
+
+ // Get the rate of angular rotation. We are inverting this because we want a
+ // positive value when we pull to the left (remember, CCW is positive in
+ // mathematics). Xbox controllers return positive values when you pull to
+ // the right by default.
+ const auto rot = -m_controller.GetX(frc::GenericHID::kRightHand) *
+ Drivetrain::kMaxAngularSpeed;
+
+ m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative);
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
new file mode 100644
index 0000000..3d44730
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/AnalogGyro.h>
+#include <frc/Encoder.h>
+#include <frc/Spark.h>
+#include <frc/controller/PIDController.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/MecanumDriveKinematics.h>
+#include <frc/kinematics/MecanumDriveOdometry.h>
+#include <frc/kinematics/MecanumDriveWheelSpeeds.h>
+#include <wpi/math>
+
+/**
+ * Represents a mecanum drive style drivetrain.
+ */
+class Drivetrain {
+ public:
+ Drivetrain() { m_gyro.Reset(); }
+
+ /**
+ * Get the robot angle as a Rotation2d
+ */
+ frc::Rotation2d GetAngle() const {
+ // Negating the angle because WPILib Gyros are CW positive.
+ return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle()));
+ }
+
+ frc::MecanumDriveWheelSpeeds GetCurrentState() const;
+ void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds);
+ void Drive(units::meters_per_second_t xSpeed,
+ units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
+ bool fieldRelative);
+ void UpdateOdometry();
+
+ static constexpr units::meters_per_second_t kMaxSpeed =
+ 3.0_mps; // 3 meters per second
+ static constexpr units::radians_per_second_t kMaxAngularSpeed{
+ wpi::math::pi}; // 1/2 rotation per second
+
+ private:
+ frc::Spark m_frontLeftMotor{1};
+ frc::Spark m_frontRightMotor{2};
+ frc::Spark m_backLeftMotor{3};
+ frc::Spark m_backRightMotor{4};
+
+ frc::Encoder m_frontLeftEncoder{0, 1};
+ frc::Encoder m_frontRightEncoder{0, 1};
+ frc::Encoder m_backLeftEncoder{0, 1};
+ frc::Encoder m_backRightEncoder{0, 1};
+
+ frc::Translation2d m_frontLeftLocation{0.381_m, 0.381_m};
+ frc::Translation2d m_frontRightLocation{0.381_m, -0.381_m};
+ frc::Translation2d m_backLeftLocation{-0.381_m, 0.381_m};
+ frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m};
+
+ frc2::PIDController m_frontLeftPIDController{1.0, 0.0, 0.0};
+ frc2::PIDController m_frontRightPIDController{1.0, 0.0, 0.0};
+ frc2::PIDController m_backLeftPIDController{1.0, 0.0, 0.0};
+ frc2::PIDController m_backRightPIDController{1.0, 0.0, 0.0};
+
+ frc::AnalogGyro m_gyro{0};
+
+ frc::MecanumDriveKinematics m_kinematics{
+ m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
+ m_backRightLocation};
+
+ frc::MecanumDriveOdometry m_odometry{m_kinematics};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp
index 78e1e96..8e0a14a 100644
--- a/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,8 +10,7 @@
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/smartdashboard/SmartDashboard.h>
-
-constexpr double kPi = 3.14159265358979;
+#include <wpi/math>
/**
* This sample program shows how to control a motor using a joystick. In the
@@ -39,7 +38,7 @@
void RobotInit() override {
// Use SetDistancePerPulse to set the multiplier for GetDistance
// This is set up assuming a 6 inch wheel with a 360 CPR encoder.
- m_encoder.SetDistancePerPulse((kPi * 6) / 360.0);
+ m_encoder.SetDistancePerPulse((wpi::math::pi * 6) / 360.0);
}
private:
diff --git a/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp
index e01a04f..afc10a3 100644
--- a/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,9 +9,9 @@
#include <frc/AnalogInput.h>
#include <frc/Joystick.h>
-#include <frc/PIDController.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
+#include <frc/controller/PIDController.h>
/**
* This is a sample program to demonstrate how to use a soft potentiometer and a
@@ -20,10 +20,6 @@
*/
class Robot : public frc::TimedRobot {
public:
- void RobotInit() override { m_pidController.SetInputRange(0, 5); }
-
- void TeleopInit() override { m_pidController.Enable(); }
-
void TeleopPeriodic() override {
// When the button is pressed once, the selected elevator setpoint is
// incremented.
@@ -35,6 +31,9 @@
m_previousButtonValue = currentButtonValue;
m_pidController.SetSetpoint(kSetPoints[m_index]);
+ double output =
+ m_pidController.Calculate(m_potentiometer.GetAverageVoltage());
+ m_elevatorMotor.Set(output);
}
private:
@@ -64,11 +63,7 @@
frc::Joystick m_joystick{kJoystickChannel};
frc::PWMVictorSPX m_elevatorMotor{kMotorChannel};
- /* Potentiometer (AnalogInput) and elevatorMotor (Victor) can be used as a
- * PIDSource and PIDOutput respectively.
- */
- frc::PIDController m_pidController{kP, kI, kD, m_potentiometer,
- m_elevatorMotor};
+ frc2::PIDController m_pidController{kP, kI, kD};
};
constexpr std::array<double, 3> Robot::kSetPoints;
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..c5d22d0
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc/trajectory/Trajectory.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/RamseteCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/button/JoystickButton.h>
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+
+ // Set up default drive command
+ m_drive.SetDefaultCommand(frc2::RunCommand(
+ [this] {
+ m_drive.ArcadeDrive(
+ m_driverController.GetY(frc::GenericHID::kLeftHand),
+ m_driverController.GetX(frc::GenericHID::kRightHand));
+ },
+ {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+
+ // While holding the shoulder button, drive at half speed
+ frc2::JoystickButton(&m_driverController, 6)
+ .WhenPressed(&m_driveHalfSpeed)
+ .WhenReleased(&m_driveFullSpeed);
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Set up config for trajectory
+ frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
+ AutoConstants::kMaxAcceleration);
+ // Add kinematics to ensure max speed is actually obeyed
+ config.SetKinematics(DriveConstants::kDriveKinematics);
+
+ // An example trajectory to follow. All units in meters.
+ auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ // Start at the origin facing the +X direction
+ frc::Pose2d(0_m, 0_m, frc::Rotation2d(0_deg)),
+ // Pass through these two interior waypoints, making an 's' curve path
+ {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
+ // End 3 meters straight ahead of where we started, facing forward
+ frc::Pose2d(3_m, 0_m, frc::Rotation2d(0_deg)),
+ // Pass the config
+ config);
+
+ frc2::RamseteCommand ramseteCommand(
+ exampleTrajectory, [this]() { return m_drive.GetPose(); },
+ frc::RamseteController(AutoConstants::kRamseteB,
+ AutoConstants::kRamseteZeta),
+ DriveConstants::ks, DriveConstants::kv, DriveConstants::ka,
+ DriveConstants::kDriveKinematics,
+ [this] {
+ return units::meters_per_second_t(m_drive.GetLeftEncoder().GetRate());
+ },
+ [this] {
+ return units::meters_per_second_t(m_drive.GetRightEncoder().GetRate());
+ },
+ frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
+ frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
+ [this](auto left, auto right) {
+ m_drive.TankDrive(left / 12_V, right / 12_V);
+ },
+ {&m_drive});
+
+ // no auto
+ return new frc2::SequentialCommandGroup(
+ std::move(ramseteCommand),
+ frc2::InstantCommand([this] { m_drive.TankDrive(0, 0); }, {}));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..3d5307f
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+#include <units/units.h>
+
+#include <frc/geometry/Rotation2d.h>
+#include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+ : m_left1{kLeftMotor1Port},
+ m_left2{kLeftMotor2Port},
+ m_right1{kRightMotor1Port},
+ m_right2{kRightMotor2Port},
+ m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
+ m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
+ m_odometry{kDriveKinematics, frc::Pose2d()} {
+ // Set the distance per pulse for the encoders
+ m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void DriveSubsystem::Periodic() {
+ // Implementation of subsystem periodic method goes here.
+ m_odometry.Update(frc::Rotation2d(units::degree_t(GetHeading())),
+ frc::DifferentialDriveWheelSpeeds{
+ units::meters_per_second_t(m_leftEncoder.GetRate()),
+ units::meters_per_second_t(m_rightEncoder.GetRate())});
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+ m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::TankDrive(double left, double right) {
+ m_drive.TankDrive(left, right, false);
+}
+
+void DriveSubsystem::ResetEncoders() {
+ m_leftEncoder.Reset();
+ m_rightEncoder.Reset();
+}
+
+double DriveSubsystem::GetAverageEncoderDistance() {
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+}
+
+frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+ m_drive.SetMaxOutput(maxOutput);
+}
+
+double DriveSubsystem::GetHeading() {
+ return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+}
+
+double DriveSubsystem::GetTurnRate() {
+ return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.);
+}
+
+frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
+
+void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
+ m_odometry.ResetPosition(pose);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
new file mode 100644
index 0000000..801e479
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <units/units.h>
+
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
+
+#pragma once
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants. This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+const int kLeftMotor1Port = 0;
+const int kLeftMotor2Port = 1;
+const int kRightMotor1Port = 2;
+const int kRightMotor2Port = 3;
+
+const int kLeftEncoderPorts[]{0, 1};
+const int kRightEncoderPorts[]{2, 3};
+const bool kLeftEncoderReversed = false;
+const bool kRightEncoderReversed = true;
+
+const auto kTrackwidth = .6_m;
+const frc::DifferentialDriveKinematics kDriveKinematics(kTrackwidth);
+
+const int kEncoderCPR = 1024;
+const double kWheelDiameterInches = 6;
+const double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+
+const bool kGyroReversed = true;
+
+// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+// These characterization values MUST be determined either experimentally or
+// theoretically for *your* robot's drive. The RobotPy Characterization
+// Toolsuite provides a convenient tool for obtaining these values for your
+// robot.
+const auto ks = 1_V;
+const auto kv = .8 * 1_V * 1_s / 1_m;
+const auto ka = .15 * 1_V * 1_s * 1_s / 1_m;
+
+// Example value only - as above, this must be tuned for your drive!
+const double kPDriveVel = .5;
+} // namespace DriveConstants
+
+namespace AutoConstants {
+const auto kMaxSpeed = 3_mps;
+const auto kMaxAcceleration = 3_mps_sq;
+
+// Reasonable baseline values for a RAMSETE follower in units of meters and
+// seconds
+const double kRamseteB = 2;
+const double kRamseteZeta = .7;
+} // namespace AutoConstants
+
+namespace OIConstants {
+const int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h
new file mode 100644
index 0000000..fa173d3
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
+
+#include "RobotContainer.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+ void RobotInit() override;
+ void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
+ void AutonomousInit() override;
+ void AutonomousPeriodic() override;
+ void TeleopInit() override;
+ void TeleopPeriodic() override;
+ void TestPeriodic() override;
+
+ private:
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h
new file mode 100644
index 0000000..cc91e0d
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/controller/PIDController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/PIDCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+
+#include "Constants.h"
+#include "subsystems/DriveSubsystem.h"
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The driver's controller
+ frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+ // The robot's subsystems and commands are defined here...
+
+ // The robot's subsystems
+ DriveSubsystem m_drive;
+
+ frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+ {}};
+ frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
+ {}};
+
+ // The chooser for the autonomous routines
+ frc::SendableChooser<frc2::Command*> m_chooser;
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..620cfd8
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/ADXRS450_Gyro.h>
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/kinematics/DifferentialDriveOdometry.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+ DriveSubsystem();
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic() override;
+
+ // Subsystem methods go here.
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ void ArcadeDrive(double fwd, double rot);
+
+ /**
+ * 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
+ */
+ void TankDrive(double left, double right);
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ void ResetEncoders();
+
+ /**
+ * Gets the average distance of the TWO encoders.
+ *
+ * @return the average of the TWO encoder readings
+ */
+ double GetAverageEncoderDistance();
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ frc::Encoder& GetLeftEncoder();
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ frc::Encoder& GetRightEncoder();
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive
+ * more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ void SetMaxOutput(double maxOutput);
+
+ /**
+ * Returns the heading of the robot.
+ *
+ * @return the robot's heading in degrees, from 180 to 180
+ */
+ double GetHeading();
+
+ /**
+ * Returns the turn rate of the robot.
+ *
+ * @return The turn rate of the robot, in degrees per second
+ */
+ double GetTurnRate();
+
+ /**
+ * Returns the currently-estimated pose of the robot.
+ *
+ * @return The pose.
+ */
+ frc::Pose2d GetPose();
+
+ /**
+ * Resets the odometry to the specified pose.
+ *
+ * @param pose The pose to which to set the odometry.
+ */
+ void ResetOdometry(frc::Pose2d pose);
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+
+ // The motor controllers
+ frc::PWMVictorSPX m_left1;
+ frc::PWMVictorSPX m_left2;
+ frc::PWMVictorSPX m_right1;
+ frc::PWMVictorSPX m_right2;
+
+ // The motors on the left side of the drive
+ frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+
+ // The motors on the right side of the drive
+ frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+
+ // The robot's drive
+ frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+
+ // The left-side drive encoder
+ frc::Encoder m_leftEncoder;
+
+ // The right-side drive encoder
+ frc::Encoder m_rightEncoder;
+
+ // The gyro sensor
+ frc::ADXRS450_Gyro m_gyro;
+
+ // Odometry class for tracking robot pose
+ frc::DifferentialDriveOdometry m_odometry;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..dbc9d2a
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/RobotContainer.cpp
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc2/command/CommandScheduler.h>
+#include <frc2/command/button/JoystickButton.h>
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // 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
+ frc2::CommandScheduler::GetInstance().OnCommandInitialize(
+ [](const frc2::Command& command) {
+ frc::Shuffleboard::AddEventMarker(
+ "Command Initialized", command.GetName(),
+ frc::ShuffleboardEventImportance::kNormal);
+ });
+ frc2::CommandScheduler::GetInstance().OnCommandInterrupt(
+ [](const frc2::Command& command) {
+ frc::Shuffleboard::AddEventMarker(
+ "Command Interrupted", command.GetName(),
+ frc::ShuffleboardEventImportance::kNormal);
+ });
+ frc2::CommandScheduler::GetInstance().OnCommandFinish(
+ [](const frc2::Command& command) {
+ frc::Shuffleboard::AddEventMarker(
+ "Command Finished", command.GetName(),
+ frc::ShuffleboardEventImportance::kNormal);
+ });
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+
+ // Run instant command 1 when the 'A' button is pressed
+ frc2::JoystickButton(&m_driverController, 0).WhenPressed(&m_instantCommand1);
+ // Run instant command 2 when the 'X' button is pressed
+ frc2::JoystickButton(&m_driverController, 3).WhenPressed(&m_instantCommand2);
+ // Run instant command 3 when the 'Y' button is held; release early to
+ // interrupt
+ frc2::JoystickButton(&m_driverController, 4).WhenHeld(&m_waitCommand);
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // no auto
+ return nullptr;
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h
new file mode 100644
index 0000000..0a5f832
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or boolean constants. This should not be used for any other
+ * purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace OIConstants {
+const int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Robot.h
new file mode 100644
index 0000000..fa173d3
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Robot.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
+
+#include "RobotContainer.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+ void RobotInit() override;
+ void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
+ void AutonomousInit() override;
+ void AutonomousPeriodic() override;
+ void TeleopInit() override;
+ void TeleopPeriodic() override;
+ void TestPeriodic() override;
+
+ private:
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/RobotContainer.h
new file mode 100644
index 0000000..6e3f321
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/RobotContainer.h
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/WaitCommand.h>
+
+#include "Constants.h"
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The robot's subsystems and commands are defined here...
+
+ // The driver's controller
+ frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+ // Three commands that do nothing; for demonstration purposes.
+ frc2::InstantCommand m_instantCommand1;
+ frc2::InstantCommand m_instantCommand2;
+ frc2::WaitCommand m_waitCommand{5_s};
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..b06845e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Run the select command in autonomous
+ return &m_exampleSelectCommand;
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
new file mode 100644
index 0000000..0a5f832
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or boolean constants. This should not be used for any other
+ * purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace OIConstants {
+const int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Robot.h
new file mode 100644
index 0000000..fa173d3
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Robot.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
+
+#include "RobotContainer.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+ void RobotInit() override;
+ void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
+ void AutonomousInit() override;
+ void AutonomousPeriodic() override;
+ void TeleopInit() override;
+ void TeleopPeriodic() override;
+ void TestPeriodic() override;
+
+ private:
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h
new file mode 100644
index 0000000..c93e320
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/Command.h>
+#include <frc2/command/PrintCommand.h>
+#include <frc2/command/SelectCommand.h>
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The enum used as keys for selecting the command to run.
+ 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.
+ CommandSelector Select() { return ONE; }
+
+ // The robot's subsystems and commands are defined here...
+
+ // An example selectcommand. Will select from the three commands based on the
+ // value returned by the selector method at runtime. Note that selectcommand
+ // takes a generic type, so the selector does not have to be an enum; it could
+ // be any desired type (string, integer, boolean, double...)
+ frc2::SelectCommand<CommandSelector> m_exampleSelectCommand{
+ [this] { return Select(); },
+ // Maps selector values to commands
+ std::pair{ONE, frc2::PrintCommand{"Command one was selected!"}},
+ std::pair{TWO, frc2::PrintCommand{"Command two was selected!"}},
+ std::pair{THREE, frc2::PrintCommand{"Command three was selected!"}}};
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
new file mode 100644
index 0000000..416d303
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Drivetrain.h"
+
+void Drivetrain::Drive(units::meters_per_second_t xSpeed,
+ units::meters_per_second_t ySpeed,
+ units::radians_per_second_t rot, bool fieldRelative) {
+ auto states = m_kinematics.ToSwerveModuleStates(
+ fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+ xSpeed, ySpeed, rot, GetAngle())
+ : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
+
+ m_kinematics.NormalizeWheelSpeeds(&states, kMaxSpeed);
+
+ auto [fl, fr, bl, br] = states;
+
+ m_frontLeft.SetDesiredState(fl);
+ m_frontRight.SetDesiredState(fr);
+ m_backLeft.SetDesiredState(bl);
+ m_backRight.SetDesiredState(br);
+}
+
+void Drivetrain::UpdateOdometry() {
+ m_odometry.Update(GetAngle(), m_frontLeft.GetState(), m_frontRight.GetState(),
+ m_backLeft.GetState(), m_backRight.GetState());
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp
new file mode 100644
index 0000000..7376e9e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+
+#include "Drivetrain.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+ void AutonomousPeriodic() override {
+ DriveWithJoystick(false);
+ m_swerve.UpdateOdometry();
+ }
+
+ void TeleopPeriodic() override { DriveWithJoystick(true); }
+
+ private:
+ frc::XboxController m_controller{0};
+ Drivetrain m_swerve;
+
+ void DriveWithJoystick(bool fieldRelative) {
+ // Get the x speed. We are inverting this because Xbox controllers return
+ // negative values when we push forward.
+ const auto xSpeed =
+ -m_controller.GetY(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed;
+
+ // Get the y speed or sideways/strafe speed. We are inverting this because
+ // we want a positive value when we pull to the left. Xbox controllers
+ // return positive values when you pull to the right by default.
+ const auto ySpeed =
+ -m_controller.GetX(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed;
+
+ // Get the rate of angular rotation. We are inverting this because we want a
+ // positive value when we pull to the left (remember, CCW is positive in
+ // mathematics). Xbox controllers return positive values when you pull to
+ // the right by default.
+ const auto rot = -m_controller.GetX(frc::GenericHID::kRightHand) *
+ Drivetrain::kMaxAngularSpeed;
+
+ m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative);
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
new file mode 100644
index 0000000..697984c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "SwerveModule.h"
+
+#include <frc/geometry/Rotation2d.h>
+#include <wpi/math>
+
+SwerveModule::SwerveModule(const int driveMotorChannel,
+ const int turningMotorChannel)
+ : m_driveMotor(driveMotorChannel), m_turningMotor(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 * wpi::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 * wpi::math::pi / kEncoderResolution);
+
+ // Limit the PID Controller's input range between -pi and pi and set the input
+ // to be continuous.
+ m_turningPIDController.EnableContinuousInput(-wpi::math::pi, wpi::math::pi);
+}
+
+frc::SwerveModuleState SwerveModule::GetState() const {
+ return {units::meters_per_second_t{m_driveEncoder.GetRate()},
+ frc::Rotation2d(units::radian_t(m_turningEncoder.Get()))};
+}
+
+void SwerveModule::SetDesiredState(const frc::SwerveModuleState& state) {
+ // Calculate the drive output from the drive PID controller.
+ const auto driveOutput = m_drivePIDController.Calculate(
+ m_driveEncoder.GetRate(), state.speed.to<double>());
+
+ // Calculate the turning motor output from the turning PID controller.
+ const auto turnOutput = m_turningPIDController.Calculate(
+ units::meter_t(m_turningEncoder.Get()),
+ // We have to convert to the meters type here because that's what
+ // ProfiledPIDController wants.
+ units::meter_t(state.angle.Radians().to<double>()));
+
+ // Set the motor outputs.
+ m_driveMotor.Set(driveOutput);
+ m_turningMotor.Set(turnOutput);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
new file mode 100644
index 0000000..745581a
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/AnalogGyro.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/kinematics/SwerveDriveOdometry.h>
+#include <wpi/math>
+
+#include "SwerveModule.h"
+
+/**
+ * Represents a swerve drive style drivetrain.
+ */
+class Drivetrain {
+ public:
+ Drivetrain() { m_gyro.Reset(); }
+
+ /**
+ * Get the robot angle as a Rotation2d.
+ */
+ frc::Rotation2d GetAngle() const {
+ // Negating the angle because WPILib Gyros are CW positive.
+ return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle()));
+ }
+
+ void Drive(units::meters_per_second_t xSpeed,
+ units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
+ bool fieldRelative);
+ void UpdateOdometry();
+
+ static constexpr units::meters_per_second_t kMaxSpeed =
+ 3.0_mps; // 3 meters per second
+ static constexpr units::radians_per_second_t kMaxAngularSpeed{
+ wpi::math::pi}; // 1/2 rotation per second
+
+ private:
+ frc::Translation2d m_frontLeftLocation{+0.381_m, +0.381_m};
+ frc::Translation2d m_frontRightLocation{+0.381_m, -0.381_m};
+ frc::Translation2d m_backLeftLocation{-0.381_m, +0.381_m};
+ frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m};
+
+ SwerveModule m_frontLeft{1, 2};
+ SwerveModule m_frontRight{2, 3};
+ SwerveModule m_backLeft{5, 6};
+ SwerveModule m_backRight{7, 8};
+
+ frc::AnalogGyro m_gyro{0};
+
+ frc::SwerveDriveKinematics<4> m_kinematics{
+ m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
+ m_backRightLocation};
+
+ frc::SwerveDriveOdometry<4> m_odometry{m_kinematics};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
new file mode 100644
index 0000000..0eaa69e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/Spark.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <wpi/math>
+
+class SwerveModule {
+ public:
+ SwerveModule(int driveMotorChannel, int turningMotorChannel);
+ frc::SwerveModuleState GetState() const;
+ void SetDesiredState(const frc::SwerveModuleState& state);
+
+ private:
+ static constexpr double kWheelRadius = 0.0508;
+ static constexpr int kEncoderResolution = 4096;
+
+ // We have to use meters here instead of radians because of the fact that
+ // ProfiledPIDController's constraints only take in meters per second and
+ // meters per second squared.
+
+ static constexpr units::meters_per_second_t kModuleMaxAngularVelocity =
+ units::meters_per_second_t(wpi::math::pi); // radians per second
+ static constexpr units::meters_per_second_squared_t
+ kModuleMaxAngularAcceleration = units::meters_per_second_squared_t(
+ wpi::math::pi * 2.0); // radians per second squared
+
+ frc::Spark m_driveMotor;
+ frc::Spark m_turningMotor;
+
+ frc::Encoder m_driveEncoder{0, 1};
+ frc::Encoder m_turningEncoder{0, 1};
+
+ frc2::PIDController m_drivePIDController{1.0, 0, 0};
+ frc::ProfiledPIDController m_turningPIDController{
+ 1.0,
+ 0.0,
+ 0.0,
+ {kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
index 67aec78..745fc10 100644
--- a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
@@ -1,15 +1,14 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <frc/AnalogInput.h>
-#include <frc/PIDController.h>
-#include <frc/PIDOutput.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
+#include <frc/controller/PIDController.h>
#include <frc/drive/DifferentialDrive.h>
/**
@@ -23,34 +22,16 @@
* ultrasonic sensor.
*/
void TeleopInit() override {
- // 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, 24 * kValueToInches);
-
// Set setpoint of the PID Controller
m_pidController.SetSetpoint(kHoldDistance * kValueToInches);
+ }
- // Begin PID control
- m_pidController.Enable();
+ void TeleopPeriodic() override {
+ double output = m_pidController.Calculate(m_ultrasonic.GetAverageVoltage());
+ m_robotDrive.ArcadeDrive(output, 0);
}
private:
- // Internal class to write to robot drive using a PIDOutput
- class MyPIDOutput : public frc::PIDOutput {
- public:
- explicit MyPIDOutput(frc::DifferentialDrive& r) : m_rd(r) {
- m_rd.SetSafetyEnabled(false);
- }
-
- void PIDWrite(double output) override {
- // Write to robot drive by reference
- m_rd.ArcadeDrive(output, 0);
- }
-
- private:
- frc::DifferentialDrive& m_rd;
- };
-
// Distance in inches the robot wants to stay from an object
static constexpr int kHoldDistance = 12;
@@ -75,9 +56,8 @@
frc::PWMVictorSPX m_left{kLeftMotorPort};
frc::PWMVictorSPX m_right{kRightMotorPort};
frc::DifferentialDrive m_robotDrive{m_left, m_right};
- MyPIDOutput m_pidOutput{m_robotDrive};
- frc::PIDController m_pidController{kP, kI, kD, m_ultrasonic, m_pidOutput};
+ frc2::PIDController m_pidController{kP, kI, kD};
};
#ifndef RUNNING_FRC_TESTS
diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json
index 8859134..864ea1d 100644
--- a/wpilibcExamples/src/main/cpp/examples/examples.json
+++ b/wpilibcExamples/src/main/cpp/examples/examples.json
@@ -167,6 +167,30 @@
"gradlebase": "cpp"
},
{
+ "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": "cpp"
+ },
+ {
+ "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": "cpp"
+ },
+ {
"name": "Getting Started",
"description": "An example program which demonstrates the simplest autonomous and teleoperated routines.",
"tags": [
@@ -208,7 +232,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, using the new command-based framework. This code can run on your computer if it supports simulation.",
"tags": [
"CommandBased Robot",
"Complete List"
@@ -243,5 +267,106 @@
],
"foldername": "ShuffleBoard",
"gradlebase": "cpp"
+ },
+ {
+ "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": "cpp"
+ },
+ {
+ "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": "cpp"
+ },
+ {
+ "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": "cpp"
+ },
+ {
+ "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": "cpp"
+ },
+ {
+ "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": "cpp"
+ },
+ {
+ "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": "cpp"
+ },
+ {
+ "name": "SwerveBot",
+ "description": "An example program for a swerve drive that uses swerve drive kinematics and odometry.",
+ "tags": [
+ "SwerveBot"
+ ],
+ "foldername": "SwerveBot",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "MecanumBot",
+ "description": "An example program for a mecanum drive that uses mecanum drive kinematics and odometry.",
+ "tags": [
+ "MecanumBot"
+ ],
+ "foldername": "MecanumBot",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "DifferentialDriveBot",
+ "description": "An example program for a differential drive that uses differential drive kinematics and odometry.",
+ "tags": [
+ "DifferentialDriveBot"
+ ],
+ "foldername": "DifferentialDriveBot",
+ "gradlebase": "cpp"
+ },
+ {
+ "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": "cpp"
}
]
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/OI.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/OI.cpp
deleted file mode 100644
index 5974f1f..0000000
--- a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/OI.cpp
+++ /dev/null
@@ -1,14 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "OI.h"
-
-#include <frc/WPILib.h>
-
-OI::OI() {
- // Process operator interface input here.
-}
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp
index df2a9cc..cd19aeb 100644
--- a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,27 +7,20 @@
#include "Robot.h"
-#include <frc/commands/Scheduler.h>
#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
-ExampleSubsystem Robot::m_subsystem;
-OI Robot::m_oi;
-
-void Robot::RobotInit() {
- m_chooser.SetDefaultOption("Default Auto", &m_defaultAuto);
- m_chooser.AddOption("My Auto", &m_myAuto);
- frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
-}
+void Robot::RobotInit() {}
/**
* This function is called every robot packet, no matter the mode. Use
- * this for items like diagnostics that you want ran during disabled,
+ * this for items like diagnostics that you want to run during disabled,
* autonomous, teleoperated and test.
*
* <p> This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
-void Robot::RobotPeriodic() {}
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
/**
* This function is called once each time the robot enters Disabled mode. You
@@ -36,36 +29,21 @@
*/
void Robot::DisabledInit() {}
-void Robot::DisabledPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+void Robot::DisabledPeriodic() {}
/**
- * This autonomous (along with the chooser code above) shows how to select
- * between different autonomous modes using the dashboard. The sendable chooser
- * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
- * remove all of the chooser code and uncomment the GetString code to get the
- * auto name from the text box below the Gyro.
- *
- * You can add additional auto modes by adding additional commands to the
- * chooser code above (like the commented example) or additional comparisons to
- * the if-else structure below with additional strings & commands.
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
*/
void Robot::AutonomousInit() {
- // std::string autoSelected = frc::SmartDashboard::GetString(
- // "Auto Selector", "Default");
- // if (autoSelected == "My Auto") {
- // m_autonomousCommand = &m_myAuto;
- // } else {
- // m_autonomousCommand = &m_defaultAuto;
- // }
-
- m_autonomousCommand = m_chooser.GetSelected();
+ m_autonomousCommand = m_container.GetAutonomousCommand();
if (m_autonomousCommand != nullptr) {
- m_autonomousCommand->Start();
+ m_autonomousCommand->Schedule();
}
}
-void Robot::AutonomousPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+void Robot::AutonomousPeriodic() {}
void Robot::TeleopInit() {
// This makes sure that the autonomous stops running when
@@ -78,8 +56,14 @@
}
}
-void Robot::TeleopPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+/**
+ * This function is called periodically during test mode.
+ */
void Robot::TestPeriodic() {}
#ifndef RUNNING_FRC_TESTS
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..8210645
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/RobotContainer.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+RobotContainer::RobotContainer() : m_autonomousCommand(&m_subsystem) {
+ // Initialize all of your commands and subsystems here
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // An example command will be run in autonomous
+ return &m_autonomousCommand;
+}
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp
index fa83682..0e709aa 100644
--- a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp
@@ -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. */
@@ -7,25 +7,5 @@
#include "commands/ExampleCommand.h"
-#include "Robot.h"
-
-ExampleCommand::ExampleCommand() {
- // Use Requires() here to declare subsystem dependencies
- Requires(&Robot::m_subsystem);
-}
-
-// Called just before this Command runs the first time
-void ExampleCommand::Initialize() {}
-
-// Called repeatedly when this Command is scheduled to run
-void ExampleCommand::Execute() {}
-
-// Make this return true when this Command no longer needs to run execute()
-bool ExampleCommand::IsFinished() { return false; }
-
-// Called once after isFinished returns true
-void ExampleCommand::End() {}
-
-// Called when another command which requires one or more of the same
-// subsystems is scheduled to run
-void ExampleCommand::Interrupted() {}
+ExampleCommand::ExampleCommand(ExampleSubsystem* subsystem)
+ : m_subsystem{subsystem} {}
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/MyAutoCommand.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/MyAutoCommand.cpp
deleted file mode 100644
index a8e9406..0000000
--- a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/MyAutoCommand.cpp
+++ /dev/null
@@ -1,31 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "commands/MyAutoCommand.h"
-
-#include "Robot.h"
-
-MyAutoCommand::MyAutoCommand() {
- // Use Requires() here to declare subsystem dependencies
- Requires(&Robot::m_subsystem);
-}
-
-// Called just before this Command runs the first time
-void MyAutoCommand::Initialize() {}
-
-// Called repeatedly when this Command is scheduled to run
-void MyAutoCommand::Execute() {}
-
-// Make this return true when this Command no longer needs to run execute()
-bool MyAutoCommand::IsFinished() { return false; }
-
-// Called once after isFinished returns true
-void MyAutoCommand::End() {}
-
-// Called when another command which requires one or more of the same
-// subsystems is scheduled to run
-void MyAutoCommand::Interrupted() {}
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp
index cdde203..2e720c9 100644
--- a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp
@@ -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. */
@@ -7,14 +7,10 @@
#include "subsystems/ExampleSubsystem.h"
-#include "RobotMap.h"
-
-ExampleSubsystem::ExampleSubsystem() : frc::Subsystem("ExampleSubsystem") {}
-
-void ExampleSubsystem::InitDefaultCommand() {
- // Set the default command for a subsystem here.
- // SetDefaultCommand(new MySpecialCommand());
+ExampleSubsystem::ExampleSubsystem() {
+ // Implementation of subsystem constructor goes here.
}
-// Put methods for controlling this subsystem
-// here. Call these from Commands.
+void ExampleSubsystem::Periodic() {
+ // Implementation of subsystem periodic method goes here.
+}
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h
new file mode 100644
index 0000000..5ac2de0
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or boolean constants. This should not be used for any other
+ * purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h
index 7dd5093..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,19 +8,12 @@
#pragma once
#include <frc/TimedRobot.h>
-#include <frc/commands/Command.h>
-#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
-#include "OI.h"
-#include "commands/ExampleCommand.h"
-#include "commands/MyAutoCommand.h"
-#include "subsystems/ExampleSubsystem.h"
+#include "RobotContainer.h"
class Robot : public frc::TimedRobot {
public:
- static ExampleSubsystem m_subsystem;
- static OI m_oi;
-
void RobotInit() override;
void RobotPeriodic() override;
void DisabledInit() override;
@@ -34,8 +27,7 @@
private:
// Have it null by default so that if testing teleop it
// doesn't have undefined behavior and potentially crash.
- frc::Command* m_autonomousCommand = nullptr;
- ExampleCommand m_defaultAuto;
- MyAutoCommand m_myAuto;
- frc::SendableChooser<frc::Command*> m_chooser;
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
};
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotContainer.h
new file mode 100644
index 0000000..46609ac
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotContainer.h
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/Command.h>
+
+#include "commands/ExampleCommand.h"
+#include "subsystems/ExampleSubsystem.h"
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The robot's subsystems and commands are defined here...
+ ExampleSubsystem m_subsystem;
+ ExampleCommand m_autonomousCommand;
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotMap.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotMap.h
deleted file mode 100644
index dd78a21..0000000
--- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotMap.h
+++ /dev/null
@@ -1,25 +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. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-/**
- * The RobotMap is a mapping from the ports sensors and actuators are wired into
- * to a variable name. This provides flexibility changing wiring, makes checking
- * the wiring easier and significantly reduces the number of magic numbers
- * floating around.
- */
-
-// For example to map the left and right motors, you could define the
-// following variables to use with your drivetrain subsystem.
-// constexpr int kLeftMotor = 1;
-// constexpr int kRightMotor = 2;
-
-// If you are using multiple modules, make sure to define both the port
-// number and the module. For example you with a rangefinder:
-// constexpr int kRangeFinderPort = 1;
-// constexpr int kRangeFinderModule = 1;
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h
index 1d11728..c36477f 100644
--- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h
@@ -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. */
@@ -7,14 +7,28 @@
#pragma once
-#include <frc/commands/Command.h>
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
-class ExampleCommand : public frc::Command {
+#include "subsystems/ExampleSubsystem.h"
+
+/**
+ * An example command that uses an example subsystem.
+ *
+ * <p>Note that this extends CommandHelper, rather extending CommandBase
+ * directly; this is crucially important, or else the decorator functions in
+ * Command will *not* work!
+ */
+class ExampleCommand
+ : public frc2::CommandHelper<frc2::CommandBase, ExampleCommand> {
public:
- ExampleCommand();
- void Initialize() override;
- void Execute() override;
- bool IsFinished() override;
- void End() override;
- void Interrupted() override;
+ /**
+ * Creates a new ExampleCommand.
+ *
+ * @param subsystem The subsystem used by this command.
+ */
+ explicit ExampleCommand(ExampleSubsystem* subsystem);
+
+ private:
+ ExampleSubsystem* m_subsystem;
};
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/MyAutoCommand.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/MyAutoCommand.h
deleted file mode 100644
index ce25e76..0000000
--- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/MyAutoCommand.h
+++ /dev/null
@@ -1,20 +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. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/Command.h>
-
-class MyAutoCommand : public frc::Command {
- public:
- MyAutoCommand();
- void Initialize() override;
- void Execute() override;
- bool IsFinished() override;
- void End() override;
- void Interrupted() override;
-};
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h
index 66bc329..763eafd 100644
--- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h
@@ -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. */
@@ -7,14 +7,20 @@
#pragma once
-#include <frc/commands/Subsystem.h>
+#include <frc2/command/SubsystemBase.h>
-class ExampleSubsystem : public frc::Subsystem {
+class ExampleSubsystem : public frc2::SubsystemBase {
public:
ExampleSubsystem();
- void InitDefaultCommand() override;
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic() override;
+
+ // Subsystem methods go here.
private:
- // It's desirable that everything possible under private except
- // for methods that implement subsystem capabilities
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
};
diff --git a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
new file mode 100644
index 0000000..8495b63
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <hal/DriverStation.h>
+
+#include <frc/DriverStation.h>
+#include <frc/livewindow/LiveWindow.h>
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <networktables/NetworkTable.h>
+
+void Robot::RobotInit() {}
+
+void Robot::Disabled() {}
+
+void Robot::Autonomous() {}
+
+void Robot::Teleop() {}
+
+void Robot::Test() {}
+
+void Robot::StartCompetition() {
+ auto& lw = *frc::LiveWindow::GetInstance();
+
+ RobotInit();
+
+ // Tell the DS that the robot is ready to be enabled
+ HAL_ObserveUserProgramStarting();
+
+ while (true) {
+ 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() && IsEnabled()) m_ds.WaitForData();
+ } else if (IsTest()) {
+ lw.SetEnabled(true);
+ frc::Shuffleboard::EnableActuatorWidgets();
+ m_ds.InTest(true);
+ Test();
+ m_ds.InTest(false);
+ while (IsTest() && IsEnabled()) m_ds.WaitForData();
+ lw.SetEnabled(false);
+ frc::Shuffleboard::DisableActuatorWidgets();
+ } else {
+ m_ds.InOperatorControl(true);
+ Teleop();
+ m_ds.InOperatorControl(false);
+ while (IsOperatorControl() && IsEnabled()) m_ds.WaitForData();
+ }
+ }
+}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/OI.h b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h
similarity index 63%
rename from wpilibcExamples/src/main/cpp/templates/commandbased/include/OI.h
rename to wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h
index 0b7713e..4efc087 100644
--- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/OI.h
+++ b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h
@@ -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,7 +7,15 @@
#pragma once
-class OI {
+#include <frc/RobotBase.h>
+
+class Robot : public frc::RobotBase {
public:
- OI();
+ void RobotInit();
+ void Disabled();
+ void Autonomous();
+ void Teleop();
+ void Test();
+
+ void StartCompetition() override;
};
diff --git a/wpilibcExamples/src/main/cpp/templates/sample/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/sample/cpp/Robot.cpp
deleted file mode 100644
index 06352ea..0000000
--- a/wpilibcExamples/src/main/cpp/templates/sample/cpp/Robot.cpp
+++ /dev/null
@@ -1,92 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "Robot.h"
-
-#include <iostream>
-
-#include <frc/Timer.h>
-#include <frc/smartdashboard/SmartDashboard.h>
-
-Robot::Robot() {
- // Note SmartDashboard is not initialized here, wait until RobotInit() to make
- // SmartDashboard calls
- m_robotDrive.SetExpiration(0.1);
-}
-
-void Robot::RobotInit() {
- m_chooser.SetDefaultOption(kAutoNameDefault, kAutoNameDefault);
- m_chooser.AddOption(kAutoNameCustom, kAutoNameCustom);
- frc::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.
- *
- * You can add additional auto modes by adding additional comparisons to the
- * if-else structure below with additional strings. If using the SendableChooser
- * make sure to add them to the chooser code above as well.
- */
-void Robot::Autonomous() {
- std::string autoSelected = m_chooser.GetSelected();
- // std::string autoSelected = frc::SmartDashboard::GetString(
- // "Auto Selector", kAutoNameDefault);
- std::cout << "Auto selected: " << autoSelected << std::endl;
-
- // 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);
-
- if (autoSelected == kAutoNameCustom) {
- // Custom Auto goes here
- std::cout << "Running custom Autonomous" << std::endl;
-
- // Spin at half speed for two seconds
- m_robotDrive.ArcadeDrive(0.0, 0.5);
- frc::Wait(2.0);
-
- // Stop robot
- m_robotDrive.ArcadeDrive(0.0, 0.0);
- } else {
- // Default Auto goes here
- std::cout << "Running default Autonomous" << std::endl;
-
- // Drive forwards at half speed for two seconds
- m_robotDrive.ArcadeDrive(-0.5, 0.0);
- frc::Wait(2.0);
-
- // Stop robot
- m_robotDrive.ArcadeDrive(0.0, 0.0);
- }
-}
-
-/**
- * Runs the motors with arcade steering.
- */
-void Robot::OperatorControl() {
- m_robotDrive.SetSafetyEnabled(true);
- while (IsOperatorControl() && IsEnabled()) {
- // Drive with arcade style (use right stick)
- m_robotDrive.ArcadeDrive(-m_stick.GetY(), m_stick.GetX());
-
- // The motors will be updated every 5ms
- frc::Wait(0.005);
- }
-}
-
-/**
- * Runs during test mode
- */
-void Robot::Test() {}
-
-#ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
-#endif
diff --git a/wpilibcExamples/src/main/cpp/templates/sample/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/sample/include/Robot.h
deleted file mode 100644
index d568111..0000000
--- a/wpilibcExamples/src/main/cpp/templates/sample/include/Robot.h
+++ /dev/null
@@ -1,49 +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. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <string>
-
-#include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/SampleRobot.h>
-#include <frc/drive/DifferentialDrive.h>
-#include <frc/smartdashboard/SendableChooser.h>
-
-/**
- * This is a demo program showing the use of the DifferentialDrive 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.
- *
- * 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.
- */
-class Robot : public frc::SampleRobot {
- public:
- Robot();
-
- void RobotInit() override;
- void Autonomous() override;
- void OperatorControl() override;
- void Test() override;
-
- private:
- // Robot drive system
- frc::PWMVictorSPX m_leftMotor{0};
- frc::PWMVictorSPX m_rightMotor{1};
- frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
-
- frc::Joystick m_stick{0};
-
- frc::SendableChooser<std::string> m_chooser;
- const std::string kAutoNameDefault = "Default";
- const std::string kAutoNameCustom = "My Auto";
-};
diff --git a/wpilibcExamples/src/main/cpp/templates/templates.json b/wpilibcExamples/src/main/cpp/templates/templates.json
index 089ea16..71798c9 100644
--- a/wpilibcExamples/src/main/cpp/templates/templates.json
+++ b/wpilibcExamples/src/main/cpp/templates/templates.json
@@ -27,6 +27,15 @@
"gradlebase": "cpp"
},
{
+ "name": "RobotBase Skeleton (Advanced)",
+ "description": "Skeleton (stub) code for RobotBase",
+ "tags": [
+ "RobotBase", "Skeleton"
+ ],
+ "foldername": "robotbaseskeleton",
+ "gradlebase": "cpp"
+ },
+ {
"name": "Command Robot",
"description": "Command style",
"tags": [
@@ -34,14 +43,5 @@
],
"foldername": "commandbased",
"gradlebase": "cpp"
- },
- {
- "name": "Sample Robot",
- "description": "Sample style",
- "tags": [
- "Sample"
- ],
- "foldername": "sample",
- "gradlebase": "cpp"
}
]