Squashed 'third_party/allwpilib_2019/' changes from 936627bd9..a3f7420da
e20d96ea4 Use __has_include for WPILib.h (#2164)
a76d006a0 Update native-utils to 2020.7.2 (#2161)
24c031d69 Increase SPI auto byte count to allow 32 bytes to be sent (#2163)
6b4eecf5f Add hidden functions to get the SPI system and SPI DMA (#2162)
ccdd0fbdb Add TrapezoidProfile external PID examples (#2131)
5c6b8a0f4 Replace std::is_pod_v with std::is_standard_layout_v (#2159)
67d2fed68 Add DutyCycleEncoder channel constructor (#2158)
d8f11eb14 Hardcode channels for LSB weight (#2153)
b2ae75acd Add way to disable "no extensions found" message (#2134)
4f951789f Build testbench tests online inorder to improve speed (#2144)
005c4c5be Try catch around task dependencies to fix loading in editor (#2156)
34f6b3f4c Fix C++ RamseteCommand param doxygen (#2157)
f7a93713f Fix up templated TrapezoidProfile classes (#2151)
8c2ff94d7 Rename MathUtils to MathUtil for consistency with other util classes (#2155)
d003ec2dc Update to 2020v9 image (#2154)
8e7cc3fe7 Add user-friendly SimDeviceSim wrappers (#2150)
6c8f6cf47 Fix bug in cubic and quintic hermetic spline generation (#2139)
e37ecd33a Sim GUI: Add support for LED displays (#2138)
57c5523d6 Fix documentation in RamseteCommand (#2149)
7b9c6ebc2 Fix wpiutilJNIShared linkage typo in wpilibj (#2143)
9a515c80f Template C++ LinearFilter to work with unit types (#2142)
5b73c17f2 Remove encoder velocities methods in DifferentialDriveOdometry (#2147)
b8c102426 Fix PS3Eye VID and PID (#2146)
2622c6c29 Add default values for b and zeta in RamseteController (#2145)
f66ae5999 Add HSV helpers to AddressableLED (#2135)
5e97c81d8 Add MedianFilter class for moving-window median (#2136)
f79b7a058 Remove unnecessary constructor arg for LinearFilter's circular buffers (#2140)
e49494830 Replace Jenkins with Azure agent (#1914)
b67d049ac Check status of PDP CAN reads (#2126)
70102a60b SendableRegistry.foreachLiveWindow: Prevent concurrent modification (#2129)
6dcd2b0e2 Improve various subsystem APIs (#2130)
ce3973435 HAL_CAN_ReceiveMessage: return MessageNotFound if no callbacks registered (#2133)
3fcfc8ea7 Fix double disable segfaulting interrupts (#2132)
6ceafe3cd Fix class reference for logger (#2123)
b058dcf64 Catch exceptions generated by OpenCV in cscore JNI (#2118)
0b9307fdf Remove unused parts of .styleguide files (#2119)
39be812b2 Fix C++ ArmFeedforward (#2120)
21e957ee4 Add DifferentialDrive voltage constraint (#2075)
e0bc97f66 Add ProfiledPIDSubsystem example (#2076)
3df44c874 Remove Rotation2d.h wpi/math include (#2117)
a58dbec8a Add holonomic follower examples (#2052)
9a8067465 Fix incomplete .styleguide (#2113)
ffa4b907c Fix C++ floating point literal formatting (#2114)
3d1ca856b Fix missing typename and return type (#2115)
5f85457a9 Add usage reporting for AddressableLED (#2108)
4ebae1712 Enforce leading/trailing zeros in Java numeric constants (#2105)
fa85fbfc1 Template C++ TrapezoidProfile and ProfiledPIDController on units (#2109)
f62e23f1a Add Doxygen for new HAL interfaces (#2110)
5443fdabc Directly construct PWM port from HAL, avoid wpilib PWM object (#2106)
c0e36df9d Standardize on PWMVictorSPX in examples (#2104)
8c4d9f541 Add TrapezoidProfileSubsystem (#2077)
45201d15f Add encoder distance overload to DifferentialDriveOdometry (#2096)
845aba33f Make feedforward classes constexpr (#2103)
500c43fb8 Add examples for DMA, DutyCycle, DutyCycleEncoder and AddressableLED (#2100)
589162811 Use DifferentialDriveWheelSpeeds in RamseteCommand ctor (#2091)
b37b68daa Add JRE deployment to MyRobot Deploy (#2099)
0e83c65d2 Fix small logic error in ParallelDeadlineGroup (#2095)
6f6c6da9f Updates to addressable LED (#2098)
1894219ef Fix devmain package in commands (#2097)
77a9949bb Add AddressableLED simulation GUI support
a4c9e4ec2 Add AddressableLED simulation support
8ed205907 Add AddressableLED (#2092)
59507b12d Bump to 2020 v7 image (#2086)
5d39bf806 Make halsimgui::DrawLEDs() values parameter const (#2088)
841ef91c0 Use gyro angle instead of robot angle for odometry (#2081)
1b66ead49 Use standard constant for pi instead of 3.14 (#2084)
db2c3dddd Use DMA Global Number for DMA Index (#2085)
82b2170fe Add DMA support to HAL and WPILibC (#2080)
8280b7e3a Add DutyCycleEncoder and AnalogEncoder (#2040)
551096006 Use kNumSystems for DutyCycle count in Ports (#2083)
df1065218 Remove release configs of deploy in MyRobot (#2082)
bf5388393 Add deploy options to myRobot (#2079)
b7bc1ea74 Update to 2020v6 image (#2078)
708009cd2 Update to gradle 6.0 (#2074)
3cce61b89 Add SmartDashboard::GetEntry function in C++ (#2064)
565e1f3e7 Fix spelling in MecanumDriveOdometry docs (#2072)
1853f7b6b Add timing window to simulation GUI
c5a049712 Add simulation pause/resume/step support
f5446c740 Add Notifier HALSIM access
3e049e02f Add name to HAL Notifier
2da64d15f Make usage reporting enums type match (#2069)
f04d95e50 Make FRCUsageReporting.h C-compatible (#2070)
d748c67a5 Generate docs for command libraries and fix doclint enable (#2071)
55a7f2b4a Add template for old command-based style (#2031)
486fa9c69 Add XboxController examples for arcade and tank drive (#2058)
e3dd1c5d7 Fix small bug in SplineHelper (#2061)
7dc7c71b5 Add feedforward components (#2045)
5f33d6af1 Fix ProfiledPIDSubsystem parameter name (#2017)
94843adb8 Standardize documentation of Speed Controllers bounds (#2043)
9bcff37b9 Add HAL specific version of wpi_setError (#2055)
326aecc9a Add error message for CAN Write Overrun (#2062)
00228678d Add requirements param to more Command APIs (#2059)
ff39a96ce Make DigitalOutput a DigitalSource (#2054)
5ccad2e8a Fix frc2::Timer returning incorrect timestamp values (#2057)
629e95776 Add VendorDeps JSON files for command libraries (#2048)
6858a57f7 Make notifier command tests a lot more lenient (#2056)
0ebe32823 Fix MyRobotStatic accidentally linking to shared command libs (#2046)
384d00f9e Fix various duty cycle bugs (#2047)
1f6850adf Add CAN Manufacturer for Studica (#2050)
7508aada9 Add ability to end startCompetition() main loop (#2032)
f5b4be16d Old C++ Command: Make GetName et al public (#2042)
e6f5c93ab Clean up new C++ commands (#2027)
39f46ceab Don't allow rising and falling values to be read from AnalogTrigger (#2039)
d93aa2b6b Add missing lock in FRCDriverStation (#2034)
114ddaf81 Fix duplicate encoders in examples (#2033)
f22d0961e Sim GUI: Add duty cycle support
3262c2bad Sim GUI: Use new multi-channel PDP getter function
96d40192a Revert accidental change to MyRobot.cpp (#2029)
ed30d5d40 Add JSON support for Trajectories (#2025)
2b6811edd Fix null pointer dereference in C++ CommandScheduler (#2023)
1d695a166 Add FPGA Duty Cycle support (#1987)
509819d83 Split the two command implementations into separate libraries (#2012)
2ad15cae1 Add multi PDP getter and sim PCM/PDP multi arg functions (#2014)
931b8ceef Add new usage reporting types from 2020v5 (#2026)
0b3821eba Change files with CRLF line endings to LF (#2022)
6f159d142 Add way to atomically check for new data, and wait otherwise (#2015)
a769f1f22 Fix bug in RamseteCommand (using degrees instead of radians) (#2020)
c5186d815 Clean up PIDCommand (#2010)
9ebd23d61 Add setVoltage method to SpeedController (#1997)
f6e311ef8 Fix SplineHelper bug (#2018)
f33bd9f05 Fix NPE in RamseteCommand (#2019)
1c1e0c9a6 Add HAL_SetAllSolenoids to sim (#2004)
ea9bb651a Remove accidental OpenCV link from wpilibc shared library (#2013)
cc0742518 Change command decorators to return implementation (#2007)
16b34cce2 Remove IterativeRobot templates (#2011)
669127e49 Update intellisense to work with Beta 2020 code (#2008)
9dc30797e Fix usage reporting indices (#2009)
f6b844ea3 Move HAL Interrupt struct to anonymous namespace (#2003)
a72f80991 Add extern C to DigitalGlitchFilterJNI (#2002)
916596cb0 Fix invalid examples json, add validator (#2001)
5509a8e96 Use constexpr for all example constants
0be6b6475 Use constexpr for DifferentialDriveKinematics
Change-Id: I1416646cbab487ad8021830215766d8ec7f24ddc
git-subtree-dir: third_party/allwpilib_2019
git-subtree-split: a3f7420dab7a104c27a0c3bf0872c999c98fd9a9
diff --git a/wpilibcExamples/.styleguide b/wpilibcExamples/.styleguide
index d9369fa..38c02cc 100644
--- a/wpilibcExamples/.styleguide
+++ b/wpilibcExamples/.styleguide
@@ -9,15 +9,16 @@
}
includeOtherLibs {
- ^HAL/
^cameraserver/
^cscore
^frc/
^frc2/
+ ^hal/
^networktables/
^ntcore
^opencv2/
^support/
+ ^units/
^vision/
^wpi/
}
diff --git a/wpilibcExamples/build.gradle b/wpilibcExamples/build.gradle
index 04a74bd..12279df 100644
--- a/wpilibcExamples/build.gradle
+++ b/wpilibcExamples/build.gradle
@@ -44,21 +44,6 @@
apply from: "${rootDir}/shared/opencv.gradle"
-ext {
- chipObjectComponents = ['commands']
- netCommComponents = ['commands']
- examplesMap.each { key, value ->
- chipObjectComponents << key.toString()
- netCommComponents << key.toString()
- }
- templatesMap.each { key, value ->
- chipObjectComponents << key.toString()
- netCommComponents << key.toString()
- }
-}
-
-apply from: "${rootDir}/shared/nilibraries.gradle"
-
model {
components {
commands(NativeLibrarySpec) {
@@ -67,6 +52,8 @@
binary.buildable = false
return
}
+ lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
+ lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
lib project: ':cscore', library: 'cscore', linkage: 'shared'
@@ -92,6 +79,8 @@
"${key}"(NativeExecutableSpec) {
targetBuildTypes 'debug'
binaries.all { binary ->
+ lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
+ lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
lib project: ':cscore', library: 'cscore', linkage: 'shared'
@@ -103,6 +92,9 @@
lib project: ':simulation:halsim_print', library: 'halsim_print', linkage: 'shared'
lib project: ':simulation:halsim_ds_nt', library: 'halsim_ds_nt', linkage: 'shared'
}
+ if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
}
sources {
cpp {
@@ -134,6 +126,8 @@
"${key}"(NativeExecutableSpec) {
targetBuildTypes 'debug'
binaries.all { binary ->
+ lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
+ lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
lib project: ':cscore', library: 'cscore', linkage: 'shared'
@@ -152,6 +146,9 @@
lib project: ':simulation:halsim_print', library: 'halsim_print', linkage: 'shared'
lib project: ':simulation:halsim_ds_nt', library: 'halsim_ds_nt', linkage: 'shared'
}
+ if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
}
sources {
cpp {
@@ -234,4 +231,7 @@
commandFile = new File("$projectDir/src/main/cpp/commands/commands.json")
}
+ext {
+ isCppCommands = true
+}
apply from: "${rootDir}/shared/examplecheck.gradle"
diff --git a/wpilibcExamples/publish.gradle b/wpilibcExamples/publish.gradle
index 927d88e..3bc1a09 100644
--- a/wpilibcExamples/publish.gradle
+++ b/wpilibcExamples/publish.gradle
@@ -12,8 +12,8 @@
def outputsFolder = file("$project.buildDir/outputs")
task cppExamplesZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = examplesZipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = examplesZipBaseName
from(licenseFile) {
into '/'
@@ -25,8 +25,8 @@
}
task cppTemplatesZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = templatesZipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = templatesZipBaseName
from(licenseFile) {
into '/'
@@ -38,8 +38,8 @@
}
task cppCommandsZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = commandsZipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = commandsZipBaseName
from(licenseFile) {
into '/'
diff --git a/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp
new file mode 100644
index 0000000..3cf84e7
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <array>
+
+#include <frc/AddressableLED.h>
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+class Robot : public frc::TimedRobot {
+ static constexpr int kLength = 60;
+
+ // PWM port 0
+ // Must be a PWM header, not MXP or DIO
+ frc::AddressableLED m_led{9};
+ std::array<frc::AddressableLED::LEDData, kLength>
+ m_ledBuffer; // Reuse the buffer
+ // Store what the last hue of the first pixel is
+ int firstPixelHue = 0;
+
+ public:
+ void Rainbow() {
+ // For every pixel
+ for (int i = 0; i < kLength; i++) {
+ // Calculate the hue - hue is easier for rainbows because the color
+ // shape is a circle so only one value needs to precess
+ const auto pixelHue = (firstPixelHue + (i * 180 / kLength)) % 180;
+ // Set the value
+ m_ledBuffer[i].SetHSV(pixelHue, 255, 128);
+ }
+ // Increase by to make the rainbow "move"
+ firstPixelHue += 3;
+ // Check bounds
+ firstPixelHue %= 180;
+ }
+
+ void RobotInit() override {
+ // Default to a length of 60, start empty output
+ // Length is expensive to set, so only set it once, then just update data
+ m_led.SetLength(kLength);
+ m_led.SetData(m_ledBuffer);
+ m_led.Start();
+ }
+
+ void RobotPeriodic() override {
+ // Fill the buffer with a rainbow
+ Rainbow();
+ // Set the LEDs
+ m_led.SetData(m_ledBuffer);
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp
new file mode 100644
index 0000000..67f1e12
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/GenericHID.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/drive/DifferentialDrive.h>
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class.
+ * Runs the motors with split arcade steering and an Xbox controller.
+ */
+class Robot : public frc::TimedRobot {
+ frc::PWMVictorSPX m_leftMotor{0};
+ frc::PWMVictorSPX m_rightMotor{1};
+ frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
+ frc::XboxController m_driverController{0};
+
+ public:
+ void TeleopPeriodic() {
+ // Drive with split arcade style
+ // That means that the Y axis of the left stick moves forward
+ // and backward, and the X of the right stick turns left and right.
+ m_robotDrive.ArcadeDrive(
+ m_driverController.GetY(frc::GenericHID::JoystickHand::kLeftHand),
+ m_driverController.GetX(frc::GenericHID::JoystickHand::kRightHand));
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..aec6b1d
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc2/command/button/JoystickButton.h>
+#include <units/units.h>
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+
+ // Set up default drive command
+ m_drive.SetDefaultCommand(frc2::RunCommand(
+ [this] {
+ m_drive.ArcadeDrive(
+ m_driverController.GetY(frc::GenericHID::kLeftHand),
+ m_driverController.GetX(frc::GenericHID::kRightHand));
+ },
+ {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+
+ // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
+ frc2::JoystickButton(&m_driverController, 1)
+ .WhenPressed([this] { m_arm.SetGoal(2_rad); }, {&m_arm});
+
+ // Move the arm to neutral position when the 'B' button is pressed.
+ frc2::JoystickButton(&m_driverController, 1)
+ .WhenPressed([this] { m_arm.SetGoal(ArmConstants::kArmOffset); },
+ {&m_arm});
+
+ // While holding the shoulder button, drive at half speed
+ frc2::JoystickButton(&m_driverController, 6)
+ .WhenPressed([this] { m_drive.SetMaxOutput(.5); })
+ .WhenReleased([this] { m_drive.SetMaxOutput(1); });
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Runs the chosen command in autonomous
+ return new frc2::InstantCommand([] {});
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp
new file mode 100644
index 0000000..5531c5b
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/ArmSubsystem.h"
+
+#include "Constants.h"
+
+using namespace ArmConstants;
+using State = frc::TrapezoidProfile<units::radians>::State;
+
+ArmSubsystem::ArmSubsystem()
+ : frc2::ProfiledPIDSubsystem<units::radians>(
+ frc::ProfiledPIDController<units::radians>(
+ kP, 0, 0, {kMaxVelocity, kMaxAcceleration})),
+ m_motor(kMotorPort),
+ m_encoder(kEncoderPorts[0], kEncoderPorts[1]),
+ m_feedforward(kS, kCos, kV, kA) {
+ m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.to<double>());
+ // Start arm in neutral position
+ SetGoal(State{kArmOffset, 0_rad_per_s});
+}
+
+void ArmSubsystem::UseOutput(double output, State setpoint) {
+ // Calculate the feedforward from the sepoint
+ units::volt_t feedforward =
+ m_feedforward.Calculate(setpoint.position, setpoint.velocity);
+ // Add the feedforward to the PID output to get the motor output
+ m_motor.SetVoltage(units::volt_t(output) + feedforward);
+}
+
+units::radian_t ArmSubsystem::GetMeasurement() {
+ return units::radian_t(m_encoder.GetDistance()) + kArmOffset;
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..64be1b8
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+ : m_left1{kLeftMotor1Port},
+ m_left2{kLeftMotor2Port},
+ m_right1{kRightMotor1Port},
+ m_right2{kRightMotor2Port},
+ m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
+ m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+ // Set the distance per pulse for the encoders
+ m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void DriveSubsystem::Periodic() {
+ // Implementation of subsystem periodic method goes here.
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+ m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::ResetEncoders() {
+ m_leftEncoder.Reset();
+ m_rightEncoder.Reset();
+}
+
+double DriveSubsystem::GetAverageEncoderDistance() {
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+}
+
+frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+ m_drive.SetMaxOutput(maxOutput);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h
new file mode 100644
index 0000000..674633c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/math>
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants. This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
+
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
+
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+} // namespace DriveConstants
+
+namespace ArmConstants {
+constexpr int kMotorPort = 4;
+
+constexpr double kP = 1;
+
+// These are fake gains; in actuality these must be determined individually for
+// each robot
+constexpr auto kS = 1_V;
+constexpr auto kCos = 1_V;
+constexpr auto kV = 0.5_V * 1_s / 1_rad;
+constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad;
+
+constexpr auto kMaxVelocity = 3_rad_per_s;
+constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s);
+
+constexpr int kEncoderPorts[]{4, 5};
+constexpr int kEncoderPPR = 256;
+constexpr auto kEncoderDistancePerPulse = 2.0_rad * wpi::math::pi / kEncoderPPR;
+
+// The offset of the arm from the horizontal in its neutral position,
+// measured from the horizontal
+constexpr auto kArmOffset = 0.5_rad;
+} // namespace ArmConstants
+
+namespace AutoConstants {
+constexpr auto kAutoTimeoutSeconds = 12_s;
+constexpr auto kAutoShootTimeSeconds = 7_s;
+} // namespace AutoConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h
similarity index 60%
rename from wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
rename to wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h
index 2c70b8f..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,15 +7,17 @@
#pragma once
-#include <string>
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
-#include <frc/IterativeRobot.h>
-#include <frc/smartdashboard/SendableChooser.h>
+#include "RobotContainer.h"
-class Robot : public frc::IterativeRobot {
+class Robot : public frc::TimedRobot {
public:
void RobotInit() override;
void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
void AutonomousInit() override;
void AutonomousPeriodic() override;
void TeleopInit() override;
@@ -23,8 +25,9 @@
void TestPeriodic() override;
private:
- frc::SendableChooser<std::string> m_chooser;
- const std::string kAutoNameDefault = "Default";
- const std::string kAutoNameCustom = "My Auto";
- std::string m_autoSelected;
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h
new file mode 100644
index 0000000..fa07359
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/ConditionalCommand.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/WaitCommand.h>
+#include <frc2/command/WaitUntilCommand.h>
+
+#include "Constants.h"
+#include "subsystems/ArmSubsystem.h"
+#include "subsystems/DriveSubsystem.h"
+
+namespace ac = AutoConstants;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The driver's controller
+ frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+ // The robot's subsystems and commands are defined here...
+
+ // The robot's subsystems
+ DriveSubsystem m_drive;
+ ArmSubsystem m_arm;
+
+ // The chooser for the autonomous routines
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h
new file mode 100644
index 0000000..693f0b4
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/controller/ArmFeedforward.h>
+#include <frc2/command/ProfiledPIDSubsystem.h>
+#include <units/units.h>
+
+/**
+ * A robot arm subsystem that moves with a motion profile.
+ */
+class ArmSubsystem : public frc2::ProfiledPIDSubsystem<units::radians> {
+ using State = frc::TrapezoidProfile<units::radians>::State;
+
+ public:
+ ArmSubsystem();
+
+ void UseOutput(double output, State setpoint) override;
+
+ units::radian_t GetMeasurement() override;
+
+ private:
+ frc::PWMVictorSPX m_motor;
+ frc::Encoder m_encoder;
+ frc::ArmFeedforward m_feedforward;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..3ed1357
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+ DriveSubsystem();
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic() override;
+
+ // Subsystem methods go here.
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ void ArcadeDrive(double fwd, double rot);
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ void ResetEncoders();
+
+ /**
+ * Gets the average distance of the TWO encoders.
+ *
+ * @return the average of the TWO encoder readings
+ */
+ double GetAverageEncoderDistance();
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ frc::Encoder& GetLeftEncoder();
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ frc::Encoder& GetRightEncoder();
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive
+ * more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ void SetMaxOutput(double maxOutput);
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+
+ // The motor controllers
+ frc::PWMVictorSPX m_left1;
+ frc::PWMVictorSPX m_left2;
+ frc::PWMVictorSPX m_right1;
+ frc::PWMVictorSPX m_right2;
+
+ // The motors on the left side of the drive
+ frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+
+ // The motors on the right side of the drive
+ frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+
+ // The robot's drive
+ frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+
+ // The left-side drive encoder
+ frc::Encoder m_leftEncoder;
+
+ // The right-side drive encoder
+ frc::Encoder m_rightEncoder;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..aec6b1d
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc2/command/button/JoystickButton.h>
+#include <units/units.h>
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+
+ // Set up default drive command
+ m_drive.SetDefaultCommand(frc2::RunCommand(
+ [this] {
+ m_drive.ArcadeDrive(
+ m_driverController.GetY(frc::GenericHID::kLeftHand),
+ m_driverController.GetX(frc::GenericHID::kRightHand));
+ },
+ {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+
+ // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
+ frc2::JoystickButton(&m_driverController, 1)
+ .WhenPressed([this] { m_arm.SetGoal(2_rad); }, {&m_arm});
+
+ // Move the arm to neutral position when the 'B' button is pressed.
+ frc2::JoystickButton(&m_driverController, 1)
+ .WhenPressed([this] { m_arm.SetGoal(ArmConstants::kArmOffset); },
+ {&m_arm});
+
+ // While holding the shoulder button, drive at half speed
+ frc2::JoystickButton(&m_driverController, 6)
+ .WhenPressed([this] { m_drive.SetMaxOutput(.5); })
+ .WhenReleased([this] { m_drive.SetMaxOutput(1); });
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Runs the chosen command in autonomous
+ return new frc2::InstantCommand([] {});
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp
new file mode 100644
index 0000000..54a59ed
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/ArmSubsystem.h"
+
+#include "Constants.h"
+
+using namespace ArmConstants;
+using State = frc::TrapezoidProfile<units::radians>::State;
+
+ArmSubsystem::ArmSubsystem()
+ : frc2::TrapezoidProfileSubsystem<units::radians>(
+ {kMaxVelocity, kMaxAcceleration}, kArmOffset),
+ m_motor(kMotorPort),
+ m_feedforward(kS, kCos, kV, kA) {
+ m_motor.SetPID(kP, 0, 0);
+}
+
+void ArmSubsystem::UseState(State setpoint) {
+ // Calculate the feedforward from the sepoint
+ units::volt_t feedforward =
+ m_feedforward.Calculate(setpoint.position, setpoint.velocity);
+ // Add the feedforward to the PID output to get the motor output
+ m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+ setpoint.position.to<double>(), feedforward / 12_V);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..47c898e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+ : m_left1{kLeftMotor1Port},
+ m_left2{kLeftMotor2Port},
+ m_right1{kRightMotor1Port},
+ m_right2{kRightMotor2Port},
+ m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
+ m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+ // Set the distance per pulse for the encoders
+ m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void DriveSubsystem::Periodic() {
+ // Implementation of subsystem periodic method goes here.
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+ m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::ResetEncoders() {
+ m_leftEncoder.Reset();
+ m_rightEncoder.Reset();
+}
+
+double DriveSubsystem::GetAverageEncoderDistance() {
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
+}
+
+frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+ m_drive.SetMaxOutput(maxOutput);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h
new file mode 100644
index 0000000..674633c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/math>
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants. This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
+
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
+
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+} // namespace DriveConstants
+
+namespace ArmConstants {
+constexpr int kMotorPort = 4;
+
+constexpr double kP = 1;
+
+// These are fake gains; in actuality these must be determined individually for
+// each robot
+constexpr auto kS = 1_V;
+constexpr auto kCos = 1_V;
+constexpr auto kV = 0.5_V * 1_s / 1_rad;
+constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad;
+
+constexpr auto kMaxVelocity = 3_rad_per_s;
+constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s);
+
+constexpr int kEncoderPorts[]{4, 5};
+constexpr int kEncoderPPR = 256;
+constexpr auto kEncoderDistancePerPulse = 2.0_rad * wpi::math::pi / kEncoderPPR;
+
+// The offset of the arm from the horizontal in its neutral position,
+// measured from the horizontal
+constexpr auto kArmOffset = 0.5_rad;
+} // namespace ArmConstants
+
+namespace AutoConstants {
+constexpr auto kAutoTimeoutSeconds = 12_s;
+constexpr auto kAutoShootTimeSeconds = 7_s;
+} // namespace AutoConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h
new file mode 100644
index 0000000..7c8d261
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/SpeedController.h>
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor
+ * controller.
+ *
+ * <p>Has no actual functionality.
+ */
+class ExampleSmartMotorController : public frc::SpeedController {
+ public:
+ enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
+
+ /**
+ * Creates a new ExampleSmartMotorController.
+ *
+ * @param port The port for the controller.
+ */
+ explicit ExampleSmartMotorController(int port) {}
+
+ /**
+ * Example method for setting the PID gains of the smart controller.
+ *
+ * @param kp The proportional gain.
+ * @param ki The integral gain.
+ * @param kd The derivative gain.
+ */
+ void SetPID(double kp, double ki, double kd) {}
+
+ /**
+ * Example method for setting the setpoint of the smart controller in PID
+ * mode.
+ *
+ * @param mode The mode of the PID controller.
+ * @param setpoint The controller setpoint.
+ * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+ */
+ void SetSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
+
+ /**
+ * Places this motor controller in follower mode.
+ *
+ * @param master The master to follow.
+ */
+ void Follow(ExampleSmartMotorController master) {}
+
+ /**
+ * Returns the encoder distance.
+ *
+ * @return The current encoder distance.
+ */
+ double GetEncoderDistance() { return 0; }
+
+ /**
+ * Returns the encoder rate.
+ *
+ * @return The current encoder rate.
+ */
+ double GetEncoderRate() { return 0; }
+
+ /**
+ * Resets the encoder to zero distance.
+ */
+ void ResetEncoder() {}
+
+ void Set(double speed) override {}
+
+ double Get() const override { return 0; }
+
+ void SetInverted(bool isInverted) override {}
+
+ bool GetInverted() const override { return false; }
+
+ void Disable() override {}
+
+ void StopMotor() override {}
+
+ void PIDWrite(double output) override {}
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h
similarity index 60%
copy from wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
copy to wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h
index 2c70b8f..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,15 +7,17 @@
#pragma once
-#include <string>
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
-#include <frc/IterativeRobot.h>
-#include <frc/smartdashboard/SendableChooser.h>
+#include "RobotContainer.h"
-class Robot : public frc::IterativeRobot {
+class Robot : public frc::TimedRobot {
public:
void RobotInit() override;
void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
void AutonomousInit() override;
void AutonomousPeriodic() override;
void TeleopInit() override;
@@ -23,8 +25,9 @@
void TestPeriodic() override;
private:
- frc::SendableChooser<std::string> m_chooser;
- const std::string kAutoNameDefault = "Default";
- const std::string kAutoNameCustom = "My Auto";
- std::string m_autoSelected;
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h
new file mode 100644
index 0000000..fa07359
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/ConditionalCommand.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/WaitCommand.h>
+#include <frc2/command/WaitUntilCommand.h>
+
+#include "Constants.h"
+#include "subsystems/ArmSubsystem.h"
+#include "subsystems/DriveSubsystem.h"
+
+namespace ac = AutoConstants;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The driver's controller
+ frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+ // The robot's subsystems and commands are defined here...
+
+ // The robot's subsystems
+ DriveSubsystem m_drive;
+ ArmSubsystem m_arm;
+
+ // The chooser for the autonomous routines
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h
new file mode 100644
index 0000000..52f15f1
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/controller/ArmFeedforward.h>
+#include <frc2/command/TrapezoidProfileSubsystem.h>
+#include <units/units.h>
+
+#include "ExampleSmartMotorController.h"
+
+/**
+ * A robot arm subsystem that moves with a motion profile.
+ */
+class ArmSubsystem : public frc2::TrapezoidProfileSubsystem<units::radians> {
+ using State = frc::TrapezoidProfile<units::radians>::State;
+
+ public:
+ ArmSubsystem();
+
+ void UseState(State setpoint) override;
+
+ private:
+ ExampleSmartMotorController m_motor;
+ frc::ArmFeedforward m_feedforward;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..3ed1357
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+ DriveSubsystem();
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic() override;
+
+ // Subsystem methods go here.
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ void ArcadeDrive(double fwd, double rot);
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ void ResetEncoders();
+
+ /**
+ * Gets the average distance of the TWO encoders.
+ *
+ * @return the average of the TWO encoder readings
+ */
+ double GetAverageEncoderDistance();
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ frc::Encoder& GetLeftEncoder();
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ frc::Encoder& GetRightEncoder();
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive
+ * more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ void SetMaxOutput(double maxOutput);
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+
+ // The motor controllers
+ frc::PWMVictorSPX m_left1;
+ frc::PWMVictorSPX m_left2;
+ frc::PWMVictorSPX m_right1;
+ frc::PWMVictorSPX m_right2;
+
+ // The motors on the left side of the drive
+ frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+
+ // The motors on the right side of the drive
+ frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+
+ // The robot's drive
+ frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+
+ // The left-side drive encoder
+ frc::Encoder m_leftEncoder;
+
+ // The right-side drive encoder
+ frc::Encoder m_rightEncoder;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp
new file mode 100644
index 0000000..a4030a5
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp
@@ -0,0 +1,82 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/AnalogInput.h>
+#include <frc/DMA.h>
+#include <frc/DMASample.h>
+#include <frc/DigitalOutput.h>
+#include <frc/Encoder.h>
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+class Robot : public frc::TimedRobot {
+ frc::DMA m_dma; // DMA object
+
+ // DMA needs a trigger, can use an output as trigger.
+ // 8 Triggers exist per DMA object, can be triggered on any
+ // DigitalSource.
+ frc::DigitalOutput m_dmaTrigger{2};
+
+ // Analog input to read with DMA
+ frc::AnalogInput m_analogInput{0};
+
+ // Encoder to read with DMA
+ frc::Encoder m_encoder{0, 1};
+
+ public:
+ void RobotInit() override {
+ // Trigger on falling edge of dma trigger output
+ m_dma.SetExternalTrigger(&m_dmaTrigger, false, true);
+
+ // Add inputs we want to read via DMA
+ m_dma.AddAnalogInput(&m_analogInput);
+ m_dma.AddEncoder(&m_encoder);
+ m_dma.AddEncoderPeriod(&m_encoder);
+
+ // Make sure trigger is set to off.
+ m_dmaTrigger.Set(true);
+
+ // Start DMA. No triggers or inputs can be added after this call
+ // unless DMA is stopped.
+ m_dma.StartDMA(1024);
+ }
+
+ void RobotPeriodic() override {
+ // Manually Trigger DMA read
+ m_dmaTrigger.Set(false);
+
+ // Need to create a sample.
+ frc::DMASample sample;
+ int32_t remaining = 0;
+ int32_t status = 0;
+ // Update our sample. remaining is the number of samples remaining in the
+ // buffer status is more specfic error messages if readStatus is not OK.
+ // Wait 1ms if buffer is empty
+ HAL_DMAReadStatus readStatus =
+ sample.Update(&m_dma, 1_ms, &remaining, &status);
+
+ if (readStatus == HAL_DMA_OK) {
+ // Status value in all these reads should be checked, a non 0 value means
+ // value could not be read
+
+ // If DMA is good, values exist
+ auto encoderDistance = sample.GetEncoderDistance(&m_encoder, &status);
+ // Period is not scaled, and is a raw value
+ auto encoderPeriod = sample.GetEncoderPeriodRaw(&m_encoder, &status);
+ auto analogVoltage =
+ sample.GetAnalogInputVoltage(&m_analogInput, &status);
+
+ frc::SmartDashboard::PutNumber("Distance", encoderDistance);
+ frc::SmartDashboard::PutNumber("Period", encoderPeriod);
+ frc::SmartDashboard::PutNumber("Input", analogVoltage);
+ }
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
index 8dcc1ea..4d9c1ed 100644
--- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
@@ -7,11 +7,6 @@
#include "Drivetrain.h"
-frc::DifferentialDriveWheelSpeeds Drivetrain::GetSpeeds() const {
- return {units::meters_per_second_t(m_leftEncoder.GetRate()),
- units::meters_per_second_t(m_rightEncoder.GetRate())};
-}
-
void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
const auto leftOutput = m_leftPIDController.Calculate(
m_leftEncoder.GetRate(), speeds.left.to<double>());
@@ -28,5 +23,6 @@
}
void Drivetrain::UpdateOdometry() {
- m_odometry.Update(GetAngle(), GetSpeeds());
+ m_odometry.Update(GetAngle(), units::meter_t(m_leftEncoder.GetDistance()),
+ units::meter_t(m_rightEncoder.GetDistance()));
}
diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
index bf2ad98..3032bd9 100644
--- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
@@ -7,15 +7,14 @@
#pragma once
-#include <units/units.h>
-
#include <frc/AnalogGyro.h>
#include <frc/Encoder.h>
-#include <frc/Spark.h>
+#include <frc/PWMVictorSPX.h>
#include <frc/SpeedControllerGroup.h>
#include <frc/controller/PIDController.h>
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/kinematics/DifferentialDriveOdometry.h>
+#include <units/units.h>
#include <wpi/math>
/**
@@ -32,6 +31,9 @@
kEncoderResolution);
m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
kEncoderResolution);
+
+ m_leftEncoder.Reset();
+ m_rightEncoder.Reset();
}
/**
@@ -47,7 +49,6 @@
static constexpr units::radians_per_second_t kMaxAngularSpeed{
wpi::math::pi}; // 1/2 rotation per second
- frc::DifferentialDriveWheelSpeeds GetSpeeds() const;
void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
void Drive(units::meters_per_second_t xSpeed,
units::radians_per_second_t rot);
@@ -58,16 +59,16 @@
static constexpr double kWheelRadius = 0.0508; // meters
static constexpr int kEncoderResolution = 4096;
- frc::Spark m_leftMaster{1};
- frc::Spark m_leftFollower{2};
- frc::Spark m_rightMaster{3};
- frc::Spark m_rightFollower{4};
+ frc::PWMVictorSPX m_leftMaster{1};
+ frc::PWMVictorSPX m_leftFollower{2};
+ frc::PWMVictorSPX m_rightMaster{3};
+ frc::PWMVictorSPX m_rightFollower{4};
frc::SpeedControllerGroup m_leftGroup{m_leftMaster, m_leftFollower};
frc::SpeedControllerGroup m_rightGroup{m_rightMaster, m_rightFollower};
frc::Encoder m_leftEncoder{0, 1};
- frc::Encoder m_rightEncoder{0, 1};
+ frc::Encoder m_rightEncoder{2, 3};
frc2::PIDController m_leftPIDController{1.0, 0.0, 0.0};
frc2::PIDController m_rightPIDController{1.0, 0.0, 0.0};
@@ -75,5 +76,5 @@
frc::AnalogGyro m_gyro{0};
frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
- frc::DifferentialDriveOdometry m_odometry{m_kinematics};
+ frc::DifferentialDriveOdometry m_odometry{GetAngle()};
};
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..78bb48a
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc2/command/button/JoystickButton.h>
+
+#include "commands/DriveDistanceProfiled.h"
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+
+ // Set up default drive command
+ m_drive.SetDefaultCommand(frc2::RunCommand(
+ [this] {
+ m_drive.ArcadeDrive(
+ m_driverController.GetY(frc::GenericHID::kLeftHand),
+ m_driverController.GetX(frc::GenericHID::kRightHand));
+ },
+ {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+
+ // While holding the shoulder button, drive at half speed
+ frc2::JoystickButton(&m_driverController, 6)
+ .WhenPressed(&m_driveHalfSpeed)
+ .WhenReleased(&m_driveFullSpeed);
+
+ // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of
+ // 10 seconds
+ frc2::JoystickButton(&m_driverController, 1)
+ .WhenPressed(DriveDistanceProfiled(3_m, &m_drive).WithTimeout(10_s));
+
+ // Do the same thing as above when the 'B' button is pressed, but defined
+ // inline
+ frc2::JoystickButton(&m_driverController, 2)
+ .WhenPressed(
+ frc2::TrapezoidProfileCommand<units::meters>(
+ frc::TrapezoidProfile<units::meters>(
+ // Limit the max acceleration and velocity
+ {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration},
+ // End at desired position in meters; implicitly starts at 0
+ {3_m, 0_mps}),
+ // Pipe the profile state to the drive
+ [this](auto setpointState) {
+ m_drive.SetDriveStates(setpointState, setpointState);
+ },
+ // Require the drive
+ {&m_drive})
+ .BeforeStarting([this]() { m_drive.ResetEncoders(); })
+ .WithTimeout(10_s));
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Runs the chosen command in autonomous
+ return new frc2::InstantCommand([] {});
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp
new file mode 100644
index 0000000..7de607c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/DriveDistanceProfiled.h"
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+DriveDistanceProfiled::DriveDistanceProfiled(units::meter_t distance,
+ DriveSubsystem* drive)
+ : CommandHelper(
+ frc::TrapezoidProfile<units::meters>(
+ // Limit the max acceleration and velocity
+ {kMaxSpeed, kMaxAcceleration},
+ // End at desired position in meters; implicitly starts at 0
+ {distance, 0_mps}),
+ // Pipe the profile state to the drive
+ [drive](auto setpointState) {
+ drive->SetDriveStates(setpointState, setpointState);
+ },
+ // Require the drive
+ {drive}) {
+ // Reset drive encoders since we're starting at 0
+ drive->ResetEncoders();
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..cd8b3e4
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+ : m_leftMaster{kLeftMotor1Port},
+ m_leftSlave{kLeftMotor2Port},
+ m_rightMaster{kRightMotor1Port},
+ m_rightSlave{kRightMotor2Port},
+ m_feedforward{ks, kv, ka} {
+ m_leftSlave.Follow(m_leftMaster);
+ m_rightSlave.Follow(m_rightMaster);
+
+ m_leftMaster.SetPID(kp, 0, 0);
+ m_rightMaster.SetPID(kp, 0, 0);
+}
+
+void DriveSubsystem::Periodic() {
+ // Implementation of subsystem periodic method goes here.
+}
+
+void DriveSubsystem::SetDriveStates(
+ frc::TrapezoidProfile<units::meters>::State left,
+ frc::TrapezoidProfile<units::meters>::State right) {
+ m_leftMaster.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+ left.position.to<double>(),
+ m_feedforward.Calculate(left.velocity) / 12_V);
+ m_rightMaster.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+ right.position.to<double>(),
+ m_feedforward.Calculate(right.velocity) / 12_V);
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+ m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::ResetEncoders() {
+ m_leftMaster.ResetEncoder();
+ m_rightMaster.ResetEncoder();
+}
+
+units::meter_t DriveSubsystem::GetLeftEncoderDistance() {
+ return units::meter_t(m_leftMaster.GetEncoderDistance());
+}
+
+units::meter_t DriveSubsystem::GetRightEncoderDistance() {
+ return units::meter_t(m_rightMaster.GetEncoderDistance());
+}
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+ m_drive.SetMaxOutput(maxOutput);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h
new file mode 100644
index 0000000..bcf3356
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/math>
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants. This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
+
+// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+// These characterization values MUST be determined either experimentally or
+// theoretically for *your* robot's drive. The RobotPy Characterization
+// Toolsuite provides a convenient tool for obtaining these values for your
+// robot.
+constexpr auto ks = 1_V;
+constexpr auto kv = 0.8_V * 1_s / 1_m;
+constexpr auto ka = 0.15_V * 1_s * 1_s / 1_m;
+
+constexpr double kp = 1;
+
+constexpr auto kMaxSpeed = 3_mps;
+constexpr auto kMaxAcceleration = 3_mps_sq;
+
+} // namespace DriveConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h
new file mode 100644
index 0000000..7c8d261
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/SpeedController.h>
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor
+ * controller.
+ *
+ * <p>Has no actual functionality.
+ */
+class ExampleSmartMotorController : public frc::SpeedController {
+ public:
+ enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
+
+ /**
+ * Creates a new ExampleSmartMotorController.
+ *
+ * @param port The port for the controller.
+ */
+ explicit ExampleSmartMotorController(int port) {}
+
+ /**
+ * Example method for setting the PID gains of the smart controller.
+ *
+ * @param kp The proportional gain.
+ * @param ki The integral gain.
+ * @param kd The derivative gain.
+ */
+ void SetPID(double kp, double ki, double kd) {}
+
+ /**
+ * Example method for setting the setpoint of the smart controller in PID
+ * mode.
+ *
+ * @param mode The mode of the PID controller.
+ * @param setpoint The controller setpoint.
+ * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+ */
+ void SetSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
+
+ /**
+ * Places this motor controller in follower mode.
+ *
+ * @param master The master to follow.
+ */
+ void Follow(ExampleSmartMotorController master) {}
+
+ /**
+ * Returns the encoder distance.
+ *
+ * @return The current encoder distance.
+ */
+ double GetEncoderDistance() { return 0; }
+
+ /**
+ * Returns the encoder rate.
+ *
+ * @return The current encoder rate.
+ */
+ double GetEncoderRate() { return 0; }
+
+ /**
+ * Resets the encoder to zero distance.
+ */
+ void ResetEncoder() {}
+
+ void Set(double speed) override {}
+
+ double Get() const override { return 0; }
+
+ void SetInverted(bool isInverted) override {}
+
+ bool GetInverted() const override { return false; }
+
+ void Disable() override {}
+
+ void StopMotor() override {}
+
+ void PIDWrite(double output) override {}
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h
similarity index 60%
copy from wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
copy to wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h
index 2c70b8f..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,15 +7,17 @@
#pragma once
-#include <string>
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
-#include <frc/IterativeRobot.h>
-#include <frc/smartdashboard/SendableChooser.h>
+#include "RobotContainer.h"
-class Robot : public frc::IterativeRobot {
+class Robot : public frc::TimedRobot {
public:
void RobotInit() override;
void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
void AutonomousInit() override;
void AutonomousPeriodic() override;
void TeleopInit() override;
@@ -23,8 +25,9 @@
void TestPeriodic() override;
private:
- frc::SendableChooser<std::string> m_chooser;
- const std::string kAutoNameDefault = "Default";
- const std::string kAutoNameCustom = "My Auto";
- std::string m_autoSelected;
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h
new file mode 100644
index 0000000..af35be4
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/StartEndCommand.h>
+
+#include "Constants.h"
+#include "subsystems/DriveSubsystem.h"
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The driver's controller
+ frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+ // The robot's subsystems and commands are defined here...
+
+ // The robot's subsystems
+ DriveSubsystem m_drive;
+
+ frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
+ {}};
+ frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
+ {}};
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h
new file mode 100644
index 0000000..2199590
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/TrapezoidProfileCommand.h>
+
+#include "subsystems/DriveSubsystem.h"
+
+class DriveDistanceProfiled
+ : public frc2::CommandHelper<frc2::TrapezoidProfileCommand<units::meters>,
+ DriveDistanceProfiled> {
+ public:
+ DriveDistanceProfiled(units::meter_t distance, DriveSubsystem* drive);
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..f68cbbd
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <frc2/command/SubsystemBase.h>
+#include <units/units.h>
+
+#include "Constants.h"
+#include "ExampleSmartMotorController.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+ DriveSubsystem();
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic() override;
+
+ // Subsystem methods go here.
+
+ /**
+ * Attempts to follow the given drive states using offboard PID.
+ *
+ * @param left The left wheel state.
+ * @param right The right wheel state.
+ */
+ void SetDriveStates(frc::TrapezoidProfile<units::meters>::State left,
+ frc::TrapezoidProfile<units::meters>::State right);
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ void ArcadeDrive(double fwd, double rot);
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ void ResetEncoders();
+
+ /**
+ * Gets the distance of the left encoder.
+ *
+ * @return the average of the TWO encoder readings
+ */
+ units::meter_t GetLeftEncoderDistance();
+
+ /**
+ * Gets the distance of the right encoder.
+ *
+ * @return the average of the TWO encoder readings
+ */
+ units::meter_t GetRightEncoderDistance();
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive
+ * more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ void SetMaxOutput(double maxOutput);
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+
+ // The motor controllers
+ ExampleSmartMotorController m_leftMaster;
+ ExampleSmartMotorController m_leftSlave;
+ ExampleSmartMotorController m_rightMaster;
+ ExampleSmartMotorController m_rightSlave;
+
+ // A feedforward component for the drive
+ frc::SimpleMotorFeedforward<units::meters> m_feedforward;
+
+ // The robot's drive
+ frc::DifferentialDrive m_drive{m_leftMaster, m_rightMaster};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp
new file mode 100644
index 0000000..b75d99c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/DutyCycleEncoder.h>
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+class Robot : public frc::TimedRobot {
+ // Duty cycle encoder on channel 0
+ frc::DutyCycleEncoder m_dutyCycleEncoder{0};
+
+ public:
+ void RobotInit() override {
+ // Set to 0.5 units per rotation
+ m_dutyCycleEncoder.SetDistancePerRotation(0.5);
+ }
+
+ void RobotPeriodic() override {
+ // Connected can be checked, and uses the frequency of the encoder
+ auto connected = m_dutyCycleEncoder.IsConnected();
+
+ // Duty Cycle Frequency in Hz
+ auto frequency = m_dutyCycleEncoder.GetFrequency();
+
+ // Output of encoder
+ auto output = m_dutyCycleEncoder.Get();
+
+ // Output scaled by DistancePerPulse
+ auto distance = m_dutyCycleEncoder.GetDistance();
+
+ frc::SmartDashboard::PutBoolean("Connected", connected);
+ frc::SmartDashboard::PutNumber("Frequency", frequency);
+ frc::SmartDashboard::PutNumber("Output", output.to<double>());
+ frc::SmartDashboard::PutNumber("Distance", distance);
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/DutyCycleInput/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DutyCycleInput/cpp/Robot.cpp
new file mode 100644
index 0000000..1a113ae
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DutyCycleInput/cpp/Robot.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/DigitalInput.h>
+#include <frc/DutyCycle.h>
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+class Robot : public frc::TimedRobot {
+ frc::DigitalInput m_input{0}; // Input channel
+ frc::DutyCycle m_dutyCycle{m_input}; // Duty cycle input
+
+ public:
+ void RobotInit() override {}
+
+ void RobotPeriodic() override {
+ // Duty Cycle Frequency in Hz
+ auto frequency = m_dutyCycle.GetFrequency();
+
+ // Output of duty cycle, ranging from 0 to 1
+ // 1 is fully on, 0 is fully off
+ auto output = m_dutyCycle.GetOutput();
+
+ frc::SmartDashboard::PutNumber("Frequency", frequency);
+ frc::SmartDashboard::PutNumber("Duty Cycle", output);
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
index 95aac42..d423dd8 100644
--- a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
@@ -5,20 +5,22 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include <units/units.h>
-
#include <frc/Encoder.h>
#include <frc/Joystick.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/trajectory/TrapezoidProfile.h>
+#include <units/units.h>
+#include <wpi/math>
class Robot : public frc::TimedRobot {
public:
static constexpr units::second_t kDt = 20_ms;
- Robot() { m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5); }
+ Robot() {
+ m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * wpi::math::pi * 1.5);
+ }
void TeleopPeriodic() override {
if (m_joystick.GetRawButtonPressed(2)) {
@@ -39,8 +41,10 @@
// Create a PID controller whose setpoint's change is subject to maximum
// velocity and acceleration constraints.
- frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq};
- frc::ProfiledPIDController m_controller{1.3, 0.0, 0.7, m_constraints, kDt};
+ frc::TrapezoidProfile<units::meters>::Constraints m_constraints{1.75_mps,
+ 0.75_mps_sq};
+ frc::ProfiledPIDController<units::meters> m_controller{1.3, 0.0, 0.7,
+ m_constraints, kDt};
};
#ifndef RUNNING_FRC_TESTS
diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
index 9a15f31..5853054 100644
--- a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
@@ -5,18 +5,23 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include <frc/Encoder.h>
#include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
-#include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/trajectory/TrapezoidProfile.h>
+#include <units/units.h>
+#include <wpi/math>
+
+#include "ExampleSmartMotorController.h"
class Robot : public frc::TimedRobot {
public:
static constexpr units::second_t kDt = 20_ms;
- Robot() { m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5); }
+ Robot() {
+ // Note: These gains are fake, and will have to be tuned for your robot.
+ m_motor.SetPID(1.3, 0.0, 0.7);
+ }
void TeleopPeriodic() override {
if (m_joystick.GetRawButtonPressed(2)) {
@@ -28,27 +33,30 @@
// Create a motion profile with the given maximum velocity and maximum
// acceleration constraints for the next setpoint, the desired goal, and the
// current setpoint.
- frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
+ frc::TrapezoidProfile<units::meter> profile{m_constraints, m_goal,
+ m_setpoint};
// Retrieve the profiled setpoint for the next timestep. This setpoint moves
// toward the goal while obeying the constraints.
m_setpoint = profile.Calculate(kDt);
- // Run controller with profiled setpoint and update motor output
- double output = m_controller.Calculate(m_encoder.GetDistance(),
- m_setpoint.position.to<double>());
- m_motor.Set(output);
+ // Send setpoint to offboard controller PID
+ m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+ m_setpoint.position.to<double>(),
+ m_feedforward.Calculate(m_setpoint.velocity) / 12_V);
}
private:
frc::Joystick m_joystick{1};
- frc::Encoder m_encoder{1, 2};
- frc::PWMVictorSPX m_motor{1};
- frc2::PIDController m_controller{1.3, 0.0, 0.7, kDt};
+ ExampleSmartMotorController m_motor{1};
+ frc::SimpleMotorFeedforward<units::meters> m_feedforward{
+ // Note: These gains are fake, and will have to be tuned for your robot.
+ 1_V, 1.5_V * 1_s / 1_m};
- frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq};
- frc::TrapezoidProfile::State m_goal;
- frc::TrapezoidProfile::State m_setpoint;
+ frc::TrapezoidProfile<units::meters>::Constraints m_constraints{1.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meters>::State m_goal;
+ frc::TrapezoidProfile<units::meters>::State m_setpoint;
};
#ifndef RUNNING_FRC_TESTS
diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h
new file mode 100644
index 0000000..7c8d261
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/SpeedController.h>
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor
+ * controller.
+ *
+ * <p>Has no actual functionality.
+ */
+class ExampleSmartMotorController : public frc::SpeedController {
+ public:
+ enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
+
+ /**
+ * Creates a new ExampleSmartMotorController.
+ *
+ * @param port The port for the controller.
+ */
+ explicit ExampleSmartMotorController(int port) {}
+
+ /**
+ * Example method for setting the PID gains of the smart controller.
+ *
+ * @param kp The proportional gain.
+ * @param ki The integral gain.
+ * @param kd The derivative gain.
+ */
+ void SetPID(double kp, double ki, double kd) {}
+
+ /**
+ * Example method for setting the setpoint of the smart controller in PID
+ * mode.
+ *
+ * @param mode The mode of the PID controller.
+ * @param setpoint The controller setpoint.
+ * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+ */
+ void SetSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
+
+ /**
+ * Places this motor controller in follower mode.
+ *
+ * @param master The master to follow.
+ */
+ void Follow(ExampleSmartMotorController master) {}
+
+ /**
+ * Returns the encoder distance.
+ *
+ * @return The current encoder distance.
+ */
+ double GetEncoderDistance() { return 0; }
+
+ /**
+ * Returns the encoder rate.
+ *
+ * @return The current encoder rate.
+ */
+ double GetEncoderRate() { return 0; }
+
+ /**
+ * Resets the encoder to zero distance.
+ */
+ void ResetEncoder() {}
+
+ void Set(double speed) override {}
+
+ double Get() const override { return 0; }
+
+ void SetInverted(bool isInverted) override {}
+
+ bool GetInverted() const override { return false; }
+
+ void Disable() override {}
+
+ void StopMotor() override {}
+
+ void PIDWrite(double output) override {}
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp
index 1dbbfa5..3cfffd7 100644
--- a/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,6 +8,7 @@
#include <frc/Encoder.h>
#include <frc/TimedRobot.h>
#include <frc/smartdashboard/SmartDashboard.h>
+#include <wpi/math>
/**
* Sample program displaying the value of a quadrature encoder on the
@@ -42,7 +43,7 @@
* inch diameter (1.5inch radius) wheel, and that we want to measure
* distance in inches.
*/
- m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);
+ m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * wpi::math::pi * 1.5);
/* Defines the lowest rate at which the encoder will not be considered
* stopped, for the purposes of the GetStopped() method. Units are in
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
index 64be1b8..47c898e 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
@@ -35,7 +35,7 @@
}
double DriveSubsystem::GetAverageEncoderDistance() {
- return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
}
frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
index d3633b6..85be752 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
@@ -17,29 +17,22 @@
: PIDSubsystem(frc2::PIDController(kP, kI, kD)),
m_shooterMotor(kShooterMotorPort),
m_feederMotor(kFeederMotorPort),
- m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]) {
- m_controller.SetTolerance(kShooterToleranceRPS);
+ m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]),
+ m_shooterFeedforward(kS, kV) {
+ m_controller.SetTolerance(kShooterToleranceRPS.to<double>());
m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ SetSetpoint(kShooterTargetRPS.to<double>());
}
-void ShooterSubsystem::UseOutput(double output) {
- // Use a feedforward of the form kS + kV * velocity
- m_shooterMotor.Set(output + kSFractional + kVFractional * kShooterTargetRPS);
-}
-
-void ShooterSubsystem::Disable() {
- // Turn off motor when we disable, since useOutput(0) doesn't stop the motor
- // due to our feedforward
- frc2::PIDSubsystem::Disable();
- m_shooterMotor.Set(0);
+void ShooterSubsystem::UseOutput(double output, double setpoint) {
+ m_shooterMotor.SetVoltage(units::volt_t(output) +
+ m_shooterFeedforward.Calculate(kShooterTargetRPS));
}
bool ShooterSubsystem::AtSetpoint() { return m_controller.AtSetpoint(); }
double ShooterSubsystem::GetMeasurement() { return m_shooterEncoder.GetRate(); }
-double ShooterSubsystem::GetSetpoint() { return kShooterTargetRPS; }
-
void ShooterSubsystem::RunFeeder() { m_feederMotor.Set(kFeederSpeed); }
void ShooterSubsystem::StopFeeder() { m_feederMotor.Set(0); }
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
index 9fbcb27..1aef3ad 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
@@ -8,6 +8,7 @@
#pragma once
#include <units/units.h>
+#include <wpi/math>
/**
* The Constants header provides a convenient place for teams to hold robot-wide
@@ -19,50 +20,50 @@
*/
namespace DriveConstants {
-const int kLeftMotor1Port = 0;
-const int kLeftMotor2Port = 1;
-const int kRightMotor1Port = 2;
-const int kRightMotor2Port = 3;
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
-const int kLeftEncoderPorts[]{0, 1};
-const int kRightEncoderPorts[]{2, 3};
-const bool kLeftEncoderReversed = false;
-const bool kRightEncoderReversed = true;
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
-const int kEncoderCPR = 1024;
-const double kWheelDiameterInches = 6;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
- (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+ (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
} // namespace DriveConstants
namespace ShooterConstants {
-const int kEncoderPorts[]{4, 5};
-const bool kEncoderReversed = false;
-const int kEncoderCPR = 1024;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderPorts[]{4, 5};
+constexpr bool kEncoderReversed = false;
+constexpr int kEncoderCPR = 1024;
+constexpr double kEncoderDistancePerPulse =
// Distance units will be rotations
- 1. / static_cast<double>(kEncoderCPR);
+ 1.0 / static_cast<double>(kEncoderCPR);
-const int kShooterMotorPort = 4;
-const int kFeederMotorPort = 5;
+constexpr int kShooterMotorPort = 4;
+constexpr int kFeederMotorPort = 5;
-const double kShooterFreeRPS = 5300;
-const double kShooterTargetRPS = 4000;
-const double kShooterToleranceRPS = 50;
+constexpr auto kShooterFreeRPS = 5300_tr / 1_s;
+constexpr auto kShooterTargetRPS = 4000_tr / 1_s;
+constexpr auto kShooterToleranceRPS = 50_tr / 1_s;
-const double kP = 1;
-const double kI = 0;
-const double kD = 0;
+constexpr double kP = 1;
+constexpr double kI = 0;
+constexpr double kD = 0;
// On a real robot the feedforward constants should be empirically determined;
// these are reasonable guesses.
-const double kSFractional = .05;
-const double kVFractional =
- // Should have value 1 at free speed...
- 1. / kShooterFreeRPS;
+constexpr auto kS = 0.05_V;
+constexpr auto kV =
+ // Should have value 12V at free speed...
+ 12_V / kShooterFreeRPS;
-const double kFeederSpeed = .5;
+constexpr double kFeederSpeed = 0.5;
} // namespace ShooterConstants
namespace AutoConstants {
@@ -71,5 +72,5 @@
} // namespace AutoConstants
namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h
index 2ce1e1c..76dff49 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h
@@ -90,7 +90,7 @@
frc2::InstantCommand m_stopFeeder{[this] { m_shooter.StopFeeder(); },
{&m_shooter}};
- frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+ frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
{}};
frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
{}};
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
index 4d30d53..e8edd28 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
@@ -9,20 +9,18 @@
#include <frc/Encoder.h>
#include <frc/PWMVictorSPX.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc2/command/PIDSubsystem.h>
+#include <units/units.h>
class ShooterSubsystem : public frc2::PIDSubsystem {
public:
ShooterSubsystem();
- void UseOutput(double output) override;
-
- double GetSetpoint() override;
+ void UseOutput(double output, double setpoint) override;
double GetMeasurement() override;
- void Disable() override;
-
bool AtSetpoint();
void RunFeeder();
@@ -33,4 +31,5 @@
frc::PWMVictorSPX m_shooterMotor;
frc::PWMVictorSPX m_feederMotor;
frc::Encoder m_shooterEncoder;
+ frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
index a34c395..acdd07c 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
@@ -28,8 +28,6 @@
double Elevator::GetMeasurement() { return m_pot.Get(); }
-void Elevator::UseOutput(double d) { m_motor.Set(d); }
-
-double Elevator::GetSetpoint() { return m_setpoint; }
-
-void Elevator::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
+void Elevator::UseOutput(double output, double setpoint) {
+ m_motor.Set(output);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
index 7f2dedc..eeb94bb 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
@@ -26,10 +26,6 @@
// frc::SmartDashboard::PutData("Wrist Angle", &m_pot);
}
-double Wrist::GetSetpoint() { return m_setpoint; }
-
-void Wrist::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
-
double Wrist::GetMeasurement() { return m_pot.Get(); }
-void Wrist::UseOutput(double d) { m_motor.Set(d); }
+void Wrist::UseOutput(double output, double setpoint) { m_motor.Set(output); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
index a2c1e87..8acecbb 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
@@ -37,14 +37,7 @@
* by
* the subsystem.
*/
- void UseOutput(double d) override;
-
- double GetSetpoint() override;
-
- /**
- * Sets the setpoint for the subsystem.
- */
- void SetSetpoint(double setpoint);
+ void UseOutput(double output, double setpoint) override;
private:
frc::PWMVictorSPX m_motor{5};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
index e3ecc69..8b54e7e 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
@@ -35,14 +35,7 @@
* by
* the subsystem.
*/
- void UseOutput(double d) override;
-
- double GetSetpoint() override;
-
- /**
- * Sets the setpoint for the subsystem.
- */
- void SetSetpoint(double setpoint);
+ void UseOutput(double output, double setpoint) override;
private:
frc::PWMVictorSPX m_motor{6};
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
index aa1117a..6213333 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
@@ -8,6 +8,9 @@
#include "RobotContainer.h"
#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc2/command/PIDCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
#include <frc2/command/button/JoystickButton.h>
RobotContainer::RobotContainer() {
@@ -29,16 +32,39 @@
void RobotContainer::ConfigureButtonBindings() {
// Configure your button bindings here
+ // Assorted commands to be bound to buttons
+
// Stabilize robot to drive straight with gyro when left bumper is held
- frc2::JoystickButton(&m_driverController, 5).WhenHeld(&m_stabilizeDriving);
+ frc2::JoystickButton(&m_driverController, 5)
+ .WhenHeld(frc2::PIDCommand{
+ frc2::PIDController{dc::kStabilizationP, dc::kStabilizationI,
+ dc::kStabilizationD},
+ // Close the loop on the turn rate
+ [this] { return m_drive.GetTurnRate(); },
+ // Setpoint is 0
+ 0,
+ // Pipe the output to the turning controls
+ [this](double output) {
+ m_drive.ArcadeDrive(m_driverController.GetY(
+ frc::GenericHID::JoystickHand::kLeftHand),
+ output);
+ },
+ // Require the robot drive
+ {&m_drive}});
// Turn to 90 degrees when the 'X' button is pressed
- frc2::JoystickButton(&m_driverController, 3).WhenPressed(&m_turnTo90);
+ frc2::JoystickButton(&m_driverController, 3)
+ .WhenPressed(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s));
+
+ // Turn to -90 degrees with a profile when the 'A' button is pressed, with a 5
+ // second timeout
+ frc2::JoystickButton(&m_driverController, 1)
+ .WhenPressed(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s));
// While holding the shoulder button, drive at half speed
frc2::JoystickButton(&m_driverController, 6)
- .WhenPressed(&m_driveHalfSpeed)
- .WhenReleased(&m_driveFullSpeed);
+ .WhenPressed(frc2::InstantCommand{[this] { m_drive.SetMaxOutput(0.5); }})
+ .WhenReleased(frc2::InstantCommand{[this] { m_drive.SetMaxOutput(1); }});
}
frc2::Command* RobotContainer::GetAutonomousCommand() {
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
index cadb7f9..988535c 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
@@ -11,12 +11,12 @@
using namespace DriveConstants;
-TurnToAngle::TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive)
+TurnToAngle::TurnToAngle(units::degree_t target, DriveSubsystem* drive)
: CommandHelper(frc2::PIDController(kTurnP, kTurnI, kTurnD),
// Close loop on heading
- [drive] { return drive->GetHeading(); },
+ [drive] { return drive->GetHeading().to<double>(); },
// Set reference to target
- targetAngleDegrees,
+ target.to<double>(),
// Pipe output to turn robot
[drive](double output) { drive->ArcadeDrive(0, output); },
// Require the drive
@@ -26,9 +26,10 @@
// Set the controller tolerance - the delta tolerance ensures the robot is
// stationary at the setpoint before it is considered as having reached the
// reference
- m_controller.SetTolerance(kTurnToleranceDeg, kTurnRateToleranceDegPerS);
+ m_controller.SetTolerance(kTurnTolerance.to<double>(),
+ kTurnRateTolerance.to<double>());
AddRequirements({drive});
}
-bool TurnToAngle::IsFinished() { return m_controller.AtSetpoint(); }
+bool TurnToAngle::IsFinished() { return GetController().AtSetpoint(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp
new file mode 100644
index 0000000..c2baa90
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/TurnToAngleProfiled.h"
+
+#include <frc/controller/ProfiledPIDController.h>
+
+using namespace DriveConstants;
+
+TurnToAngleProfiled::TurnToAngleProfiled(units::degree_t target,
+ DriveSubsystem* drive)
+ : CommandHelper(
+ frc::ProfiledPIDController<units::radians>(
+ kTurnP, kTurnI, kTurnD, {kMaxTurnRate, kMaxTurnAcceleration}),
+ // Close loop on heading
+ [drive] { return drive->GetHeading(); },
+ // Set reference to target
+ target,
+ // Pipe output to turn robot
+ [drive](double output, auto setpointState) {
+ drive->ArcadeDrive(0, output);
+ },
+ // Require the drive
+ {drive}) {
+ // Set the controller to be continuous (because it is an angle controller)
+ GetController().EnableContinuousInput(-180_deg, 180_deg);
+ // Set the controller tolerance - the delta tolerance ensures the robot is
+ // stationary at the setpoint before it is considered as having reached the
+ // reference
+ GetController().SetTolerance(kTurnTolerance, kTurnRateTolerance);
+
+ AddRequirements({drive});
+}
+
+bool TurnToAngleProfiled::IsFinished() { return GetController().AtGoal(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
index 7e5ef96..c6d8d8c 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
@@ -35,7 +35,7 @@
}
double DriveSubsystem::GetAverageEncoderDistance() {
- return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
}
frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
@@ -46,10 +46,11 @@
m_drive.SetMaxOutput(maxOutput);
}
-double DriveSubsystem::GetHeading() {
- return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+units::degree_t DriveSubsystem::GetHeading() {
+ return units::degree_t(std::remainder(m_gyro.GetAngle(), 360) *
+ (kGyroReversed ? -1.0 : 1.0));
}
double DriveSubsystem::GetTurnRate() {
- return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.);
+ return m_gyro.GetRate() * (kGyroReversed ? -1.0 : 1.0);
}
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
index 0349c71..f423640 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
@@ -7,6 +7,9 @@
#pragma once
+#include <units/units.h>
+#include <wpi/math>
+
/**
* The Constants header provides a convenient place for teams to hold robot-wide
* numerical or bool constants. This should not be used for any other purpose.
@@ -17,42 +20,45 @@
*/
namespace DriveConstants {
-const int kLeftMotor1Port = 0;
-const int kLeftMotor2Port = 1;
-const int kRightMotor1Port = 2;
-const int kRightMotor2Port = 3;
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
-const int kLeftEncoderPorts[]{0, 1};
-const int kRightEncoderPorts[]{2, 3};
-const bool kLeftEncoderReversed = false;
-const bool kRightEncoderReversed = true;
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
-const int kEncoderCPR = 1024;
-const double kWheelDiameterInches = 6;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
- (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+ (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
-const bool kGyroReversed = true;
+constexpr bool kGyroReversed = true;
-const double kStabilizationP = 1;
-const double kStabilizationI = .5;
-const double kStabilizationD = 0;
+constexpr double kStabilizationP = 1;
+constexpr double kStabilizationI = 0.5;
+constexpr double kStabilizationD = 0;
-const double kTurnP = 1;
-const double kTurnI = 0;
-const double kTurnD = 0;
+constexpr double kTurnP = 1;
+constexpr double kTurnI = 0;
+constexpr double kTurnD = 0;
-const double kTurnToleranceDeg = 5;
-const double kTurnRateToleranceDegPerS = 10; // degrees per second
+constexpr auto kTurnTolerance = 5_deg;
+constexpr auto kTurnRateTolerance = 10_deg_per_s;
+
+constexpr auto kMaxTurnRate = 100_deg_per_s;
+constexpr auto kMaxTurnAcceleration = 300_deg_per_s / 1_s;
} // namespace DriveConstants
namespace AutoConstants {
-const double kAutoDriveDistanceInches = 60;
-const double kAutoBackupDistanceInches = 20;
-const double kAutoDriveSpeed = .5;
+constexpr double kAutoDriveDistanceInches = 60;
+constexpr double kAutoBackupDistanceInches = 20;
+constexpr double kAutoDriveSpeed = 0.5;
} // namespace AutoConstants
namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
index f480536..37c7e4f 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
@@ -12,9 +12,6 @@
#include <frc/smartdashboard/SendableChooser.h>
#include <frc2/command/Command.h>
#include <frc2/command/InstantCommand.h>
-#include <frc2/command/PIDCommand.h>
-#include <frc2/command/ParallelRaceGroup.h>
-#include <frc2/command/RunCommand.h>
#include "Constants.h"
#include "commands/TurnToAngle.h"
@@ -45,34 +42,6 @@
// The robot's subsystems
DriveSubsystem m_drive;
- // Assorted commands to be bound to buttons
-
- // Turn to 90 degrees, with a 5 second timeout
- frc2::ParallelRaceGroup m_turnTo90 =
- TurnToAngle{90, &m_drive}.WithTimeout(5_s);
-
- // Stabilize the robot while driving
- frc2::PIDCommand m_stabilizeDriving{
- frc2::PIDController{dc::kStabilizationP, dc::kStabilizationI,
- dc::kStabilizationD},
- // Close the loop on the turn rate
- [this] { return m_drive.GetTurnRate(); },
- // Setpoint is 0
- 0,
- // Pipe the output to the turning controls
- [this](double output) {
- m_drive.ArcadeDrive(
- m_driverController.GetY(frc::GenericHID::JoystickHand::kLeftHand),
- output);
- },
- // Require the robot drive
- {&m_drive}};
-
- frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
- {}};
- frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
- {}};
-
// The chooser for the autonomous routines
frc::SendableChooser<frc2::Command*> m_chooser;
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h
index c7e875e..4afa20e 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h
@@ -23,7 +23,7 @@
* @param targetAngleDegrees The angle to turn to
* @param drive The drive subsystem to use
*/
- TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive);
+ TurnToAngle(units::degree_t target, DriveSubsystem* drive);
bool IsFinished() override;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h
new file mode 100644
index 0000000..0b52044
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/ProfiledPIDCommand.h>
+
+#include "subsystems/DriveSubsystem.h"
+
+/**
+ * A command that will turn the robot to the specified angle using a motion
+ * profile.
+ */
+class TurnToAngleProfiled
+ : public frc2::CommandHelper<frc2::ProfiledPIDCommand<units::radians>,
+ TurnToAngleProfiled> {
+ public:
+ /**
+ * Turns to robot to the specified angle using a motion profile.
+ *
+ * @param targetAngleDegrees The angle to turn to
+ * @param drive The drive subsystem to use
+ */
+ TurnToAngleProfiled(units::degree_t targetAngleDegrees,
+ DriveSubsystem* drive);
+
+ bool IsFinished() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
index cada816..c4cd21c 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
@@ -13,6 +13,7 @@
#include <frc/SpeedControllerGroup.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc2/command/SubsystemBase.h>
+#include <units/units.h>
#include "Constants.h"
@@ -74,7 +75,7 @@
*
* @return the robot's heading in degrees, from 180 to 180
*/
- double GetHeading();
+ units::degree_t GetHeading();
/**
* Returns the turn rate of the robot.
diff --git a/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c b/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c
index dedb0f2..ae5833e 100644
--- a/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c
+++ b/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -18,7 +18,7 @@
#include <stdio.h>
-#include "hal/HAL.h"
+#include <hal/HAL.h>
enum DriverStationMode {
DisabledMode,
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
index 64be1b8..47c898e 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
@@ -35,7 +35,7 @@
}
double DriveSubsystem::GetAverageEncoderDistance() {
- return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
}
frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
index b09572e..d465f6c 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
@@ -7,6 +7,8 @@
#pragma once
+#include <wpi/math>
+
/**
* The Constants header provides a convenient place for teams to hold robot-wide
* numerical or bool constants. This should not be used for any other purpose.
@@ -17,34 +19,34 @@
*/
namespace DriveConstants {
-const int kLeftMotor1Port = 0;
-const int kLeftMotor2Port = 1;
-const int kRightMotor1Port = 2;
-const int kRightMotor2Port = 3;
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
-const int kLeftEncoderPorts[]{0, 1};
-const int kRightEncoderPorts[]{2, 3};
-const bool kLeftEncoderReversed = false;
-const bool kRightEncoderReversed = true;
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
-const int kEncoderCPR = 1024;
-const double kWheelDiameterInches = 6;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
- (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+ (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
} // namespace DriveConstants
namespace HatchConstants {
-const int kHatchSolenoidModule = 0;
-const int kHatchSolenoidPorts[]{0, 1};
+constexpr int kHatchSolenoidModule = 0;
+constexpr int kHatchSolenoidPorts[]{0, 1};
} // namespace HatchConstants
namespace AutoConstants {
-const double kAutoDriveDistanceInches = 60;
-const double kAutoBackupDistanceInches = 20;
-const double kAutoDriveSpeed = .5;
+constexpr double kAutoDriveDistanceInches = 60;
+constexpr double kAutoBackupDistanceInches = 20;
+constexpr double kAutoDriveSpeed = 0.5;
} // namespace AutoConstants
namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h
index b7c57a3..106812b 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h
@@ -63,7 +63,7 @@
frc2::InstantCommand m_grabHatch{[this] { m_hatch.GrabHatch(); }, {&m_hatch}};
frc2::InstantCommand m_releaseHatch{[this] { m_hatch.ReleaseHatch(); },
{&m_hatch}};
- frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+ frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
{}};
frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
{}};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp
index 839bb87..b35b2c6 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp
@@ -10,6 +10,6 @@
HalveDriveSpeed::HalveDriveSpeed(DriveSubsystem* subsystem)
: m_drive(subsystem) {}
-void HalveDriveSpeed::Initialize() { m_drive->SetMaxOutput(.5); }
+void HalveDriveSpeed::Initialize() { m_drive->SetMaxOutput(0.5); }
void HalveDriveSpeed::End(bool interrupted) { m_drive->SetMaxOutput(1); }
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
index 64be1b8..47c898e 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
@@ -35,7 +35,7 @@
}
double DriveSubsystem::GetAverageEncoderDistance() {
- return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
}
frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
index b09572e..d465f6c 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
@@ -7,6 +7,8 @@
#pragma once
+#include <wpi/math>
+
/**
* The Constants header provides a convenient place for teams to hold robot-wide
* numerical or bool constants. This should not be used for any other purpose.
@@ -17,34 +19,34 @@
*/
namespace DriveConstants {
-const int kLeftMotor1Port = 0;
-const int kLeftMotor2Port = 1;
-const int kRightMotor1Port = 2;
-const int kRightMotor2Port = 3;
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
-const int kLeftEncoderPorts[]{0, 1};
-const int kRightEncoderPorts[]{2, 3};
-const bool kLeftEncoderReversed = false;
-const bool kRightEncoderReversed = true;
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
-const int kEncoderCPR = 1024;
-const double kWheelDiameterInches = 6;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
- (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+ (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
} // namespace DriveConstants
namespace HatchConstants {
-const int kHatchSolenoidModule = 0;
-const int kHatchSolenoidPorts[]{0, 1};
+constexpr int kHatchSolenoidModule = 0;
+constexpr int kHatchSolenoidPorts[]{0, 1};
} // namespace HatchConstants
namespace AutoConstants {
-const double kAutoDriveDistanceInches = 60;
-const double kAutoBackupDistanceInches = 20;
-const double kAutoDriveSpeed = .5;
+constexpr double kAutoDriveDistanceInches = 60;
+constexpr double kAutoBackupDistanceInches = 20;
+constexpr double kAutoDriveSpeed = 0.5;
} // namespace AutoConstants
namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
index 3d44730..6550085 100644
--- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
@@ -9,7 +9,7 @@
#include <frc/AnalogGyro.h>
#include <frc/Encoder.h>
-#include <frc/Spark.h>
+#include <frc/PWMVictorSPX.h>
#include <frc/controller/PIDController.h>
#include <frc/geometry/Translation2d.h>
#include <frc/kinematics/MecanumDriveKinematics.h>
@@ -45,15 +45,15 @@
wpi::math::pi}; // 1/2 rotation per second
private:
- frc::Spark m_frontLeftMotor{1};
- frc::Spark m_frontRightMotor{2};
- frc::Spark m_backLeftMotor{3};
- frc::Spark m_backRightMotor{4};
+ frc::PWMVictorSPX m_frontLeftMotor{1};
+ frc::PWMVictorSPX m_frontRightMotor{2};
+ frc::PWMVictorSPX m_backLeftMotor{3};
+ frc::PWMVictorSPX m_backRightMotor{4};
frc::Encoder m_frontLeftEncoder{0, 1};
- frc::Encoder m_frontRightEncoder{0, 1};
- frc::Encoder m_backLeftEncoder{0, 1};
- frc::Encoder m_backRightEncoder{0, 1};
+ frc::Encoder m_frontRightEncoder{2, 3};
+ frc::Encoder m_backLeftEncoder{4, 5};
+ frc::Encoder m_backRightEncoder{6, 7};
frc::Translation2d m_frontLeftLocation{0.381_m, 0.381_m};
frc::Translation2d m_frontRightLocation{0.381_m, -0.381_m};
@@ -71,5 +71,5 @@
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
m_backRightLocation};
- frc::MecanumDriveOdometry m_odometry{m_kinematics};
+ frc::MecanumDriveOdometry m_odometry{m_kinematics, GetAngle()};
};
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..c7379cb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/controller/PIDController.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc/trajectory/Trajectory.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/MecanumControllerCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/button/JoystickButton.h>
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+const frc::MecanumDriveKinematics DriveConstants::kDriveKinematics{
+ frc::Translation2d(kWheelBase / 2, kTrackWidth / 2),
+ frc::Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+ frc::Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+ frc::Translation2d(-kWheelBase / 2, -kTrackWidth / 2)};
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+
+ // Set up default drive command
+ m_drive.SetDefaultCommand(frc2::RunCommand(
+ [this] {
+ m_drive.Drive(m_driverController.GetY(frc::GenericHID::kLeftHand),
+ m_driverController.GetX(frc::GenericHID::kRightHand),
+ m_driverController.GetX(frc::GenericHID::kLeftHand),
+ false);
+ },
+ {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+ // Configure your button bindings here
+
+ // While holding the shoulder button, drive at half speed
+ frc2::JoystickButton(&m_driverController, 6)
+ .WhenPressed(&m_driveHalfSpeed)
+ .WhenReleased(&m_driveFullSpeed);
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Set up config for trajectory
+ frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
+ AutoConstants::kMaxAcceleration);
+ // Add kinematics to ensure max speed is actually obeyed
+ config.SetKinematics(DriveConstants::kDriveKinematics);
+
+ // An example trajectory to follow. All units in meters.
+ auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ // Start at the origin facing the +X direction
+ frc::Pose2d(0_m, 0_m, frc::Rotation2d(0_deg)),
+ // Pass through these two interior waypoints, making an 's' curve path
+ {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
+ // End 3 meters straight ahead of where we started, facing forward
+ frc::Pose2d(3_m, 0_m, frc::Rotation2d(0_deg)),
+ // Pass the config
+ config);
+
+ frc2::MecanumControllerCommand mecanumControllerCommand(
+ exampleTrajectory, [this]() { return m_drive.GetPose(); },
+
+ frc::SimpleMotorFeedforward<units::meters>(ks, kv, ka),
+ DriveConstants::kDriveKinematics,
+
+ frc2::PIDController(AutoConstants::kPXController, 0, 0),
+ frc2::PIDController(AutoConstants::kPYController, 0, 0),
+ frc::ProfiledPIDController<units::radians>(
+ AutoConstants::kPThetaController, 0, 0,
+ AutoConstants::kThetaControllerConstraints),
+
+ AutoConstants::kMaxSpeed,
+
+ [this]() {
+ return frc::MecanumDriveWheelSpeeds{
+ units::meters_per_second_t(m_drive.GetFrontLeftEncoder().GetRate()),
+ units::meters_per_second_t(
+ m_drive.GetFrontRightEncoder().GetRate()),
+ units::meters_per_second_t(m_drive.GetRearLeftEncoder().GetRate()),
+ units::meters_per_second_t(
+ m_drive.GetRearRightEncoder().GetRate())};
+ },
+
+ frc2::PIDController(DriveConstants::kPFrontLeftVel, 0, 0),
+ frc2::PIDController(DriveConstants::kPRearLeftVel, 0, 0),
+ frc2::PIDController(DriveConstants::kPFrontRightVel, 0, 0),
+ frc2::PIDController(DriveConstants::kPRearRightVel, 0, 0),
+
+ [this](units::volt_t frontLeft, units::volt_t rearLeft,
+ units::volt_t frontRight, units::volt_t rearRight) {
+ m_drive.SetSpeedControllersVolts(frontLeft, rearLeft, frontRight,
+ rearRight);
+ },
+
+ {&m_drive});
+
+ // no auto
+ return new frc2::SequentialCommandGroup(
+ std::move(mecanumControllerCommand),
+ frc2::InstantCommand([this]() { m_drive.Drive(0, 0, 0, false); }, {}));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..b758b07
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+#include <frc/geometry/Rotation2d.h>
+#include <units/units.h>
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+ : m_frontLeft{kFrontLeftMotorPort},
+ m_rearLeft{kRearLeftMotorPort},
+ m_frontRight{kFrontRightMotorPort},
+ m_rearRight{kRearRightMotorPort},
+
+ m_frontLeftEncoder{kFrontLeftEncoderPorts[0], kFrontLeftEncoderPorts[1],
+ kFrontLeftEncoderReversed},
+ m_rearLeftEncoder{kRearLeftEncoderPorts[0], kRearLeftEncoderPorts[1],
+ kRearLeftEncoderReversed},
+ m_frontRightEncoder{kFrontRightEncoderPorts[0],
+ kFrontRightEncoderPorts[1],
+ kFrontRightEncoderReversed},
+ m_rearRightEncoder{kRearRightEncoderPorts[0], kRearRightEncoderPorts[1],
+ kRearRightEncoderReversed},
+
+ m_odometry{kDriveKinematics,
+ frc::Rotation2d(units::degree_t(GetHeading())),
+ frc::Pose2d()} {
+ // Set the distance per pulse for the encoders
+ m_frontLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ m_rearLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ m_frontRightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+ m_rearRightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void DriveSubsystem::Periodic() {
+ // Implementation of subsystem periodic method goes here.
+ m_odometry.Update(
+ frc::Rotation2d(units::degree_t(GetHeading())),
+ frc::MecanumDriveWheelSpeeds{
+ units::meters_per_second_t(m_frontLeftEncoder.GetRate()),
+ units::meters_per_second_t(m_rearLeftEncoder.GetRate()),
+ units::meters_per_second_t(m_frontRightEncoder.GetRate()),
+ units::meters_per_second_t(m_rearRightEncoder.GetRate())});
+}
+
+void DriveSubsystem::Drive(double xSpeed, double ySpeed, double rot,
+ bool feildRelative) {
+ if (feildRelative) {
+ m_drive.DriveCartesian(ySpeed, xSpeed, rot, -m_gyro.GetAngle());
+ } else {
+ m_drive.DriveCartesian(ySpeed, xSpeed, rot);
+ }
+}
+
+void DriveSubsystem::SetSpeedControllersVolts(units::volt_t frontLeftPower,
+ units::volt_t rearLeftPower,
+ units::volt_t frontRightPower,
+ units::volt_t rearRightPower) {
+ m_frontLeft.SetVoltage(frontLeftPower);
+ m_rearLeft.SetVoltage(rearLeftPower);
+ m_frontRight.SetVoltage(frontRightPower);
+ m_rearRight.SetVoltage(rearRightPower);
+}
+
+void DriveSubsystem::ResetEncoders() {
+ m_frontLeftEncoder.Reset();
+ m_rearLeftEncoder.Reset();
+ m_frontRightEncoder.Reset();
+ m_rearRightEncoder.Reset();
+}
+
+frc::Encoder& DriveSubsystem::GetFrontLeftEncoder() {
+ return m_frontLeftEncoder;
+}
+
+frc::Encoder& DriveSubsystem::GetRearLeftEncoder() { return m_rearLeftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetFrontRightEncoder() {
+ return m_frontRightEncoder;
+}
+
+frc::Encoder& DriveSubsystem::GetRearRightEncoder() {
+ return m_rearRightEncoder;
+}
+
+frc::MecanumDriveWheelSpeeds DriveSubsystem::getCurrentWheelSpeeds() {
+ return (frc::MecanumDriveWheelSpeeds{
+ units::meters_per_second_t(m_frontLeftEncoder.GetRate()),
+ units::meters_per_second_t(m_rearLeftEncoder.GetRate()),
+ units::meters_per_second_t(m_frontRightEncoder.GetRate()),
+ units::meters_per_second_t(m_rearRightEncoder.GetRate())});
+}
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+ m_drive.SetMaxOutput(maxOutput);
+}
+
+double DriveSubsystem::GetHeading() {
+ return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+}
+
+void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); }
+
+double DriveSubsystem::GetTurnRate() {
+ return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.);
+}
+
+frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
+
+void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
+ m_odometry.ResetPosition(pose,
+ frc::Rotation2d(units::degree_t(GetHeading())));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
new file mode 100644
index 0000000..5f5f05f
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/MecanumDriveKinematics.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <units/units.h>
+#include <wpi/math>
+
+#pragma once
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants. This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+constexpr int kFrontLeftMotorPort = 0;
+constexpr int kRearLeftMotorPort = 1;
+constexpr int kFrontRightMotorPort = 2;
+constexpr int kRearRightMotorPort = 3;
+
+constexpr int kFrontLeftEncoderPorts[]{0, 1};
+constexpr int kRearLeftEncoderPorts[]{2, 3};
+constexpr int kFrontRightEncoderPorts[]{4, 5};
+constexpr int kRearRightEncoderPorts[]{5, 6};
+
+constexpr bool kFrontLeftEncoderReversed = false;
+constexpr bool kRearLeftEncoderReversed = true;
+constexpr bool kFrontRightEncoderReversed = false;
+constexpr bool kRearRightEncoderReversed = true;
+
+constexpr auto kTrackWidth =
+ 0.5_m; // Distance between centers of right and left wheels on robot
+constexpr auto kWheelBase =
+ 0.7_m; // Distance between centers of front and back wheels on robot
+extern const frc::MecanumDriveKinematics kDriveKinematics;
+
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterMeters = .15;
+constexpr double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterMeters * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+
+constexpr bool kGyroReversed = false;
+
+// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+// These characterization values MUST be determined either experimentally or
+// theoretically for *your* robot's drive. The RobotPy Characterization
+// Toolsuite provides a convenient tool for obtaining these values for your
+// robot.
+constexpr auto ks = 1_V;
+constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
+constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m;
+
+// Example value only - as above, this must be tuned for your drive!
+constexpr double kPFrontLeftVel = 0.5;
+constexpr double kPRearLeftVel = 0.5;
+constexpr double kPFrontRightVel = 0.5;
+constexpr double kPRearRightVel = 0.5;
+} // namespace DriveConstants
+
+namespace AutoConstants {
+using radians_per_second_squared_t =
+ units::compound_unit<units::radians,
+ units::inverse<units::squared<units::second>>>;
+
+constexpr auto kMaxSpeed = units::meters_per_second_t(3);
+constexpr auto kMaxAcceleration = units::meters_per_second_squared_t(3);
+constexpr auto kMaxAngularSpeed = units::radians_per_second_t(3);
+constexpr auto kMaxAngularAcceleration =
+ units::unit_t<radians_per_second_squared_t>(3);
+
+constexpr double kPXController = 0.5;
+constexpr double kPYController = 0.5;
+constexpr double kPThetaController = 0.5;
+
+constexpr frc::TrapezoidProfile<units::radians>::Constraints
+ kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration};
+
+} // namespace AutoConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h
similarity index 60%
copy from wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
copy to wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h
index 2c70b8f..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,15 +7,17 @@
#pragma once
-#include <string>
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
-#include <frc/IterativeRobot.h>
-#include <frc/smartdashboard/SendableChooser.h>
+#include "RobotContainer.h"
-class Robot : public frc::IterativeRobot {
+class Robot : public frc::TimedRobot {
public:
void RobotInit() override;
void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
void AutonomousInit() override;
void AutonomousPeriodic() override;
void TeleopInit() override;
@@ -23,8 +25,9 @@
void TestPeriodic() override;
private:
- frc::SendableChooser<std::string> m_chooser;
- const std::string kAutoNameDefault = "Default";
- const std::string kAutoNameCustom = "My Auto";
- std::string m_autoSelected;
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h
new file mode 100644
index 0000000..3b4f6cc
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/PIDCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+
+#include "Constants.h"
+#include "subsystems/DriveSubsystem.h"
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The driver's controller
+ frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+ // The robot's subsystems and commands are defined here...
+
+ // The robot's subsystems
+ DriveSubsystem m_drive;
+
+ frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+ {}};
+ frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
+ {}};
+
+ // The chooser for the autonomous routines
+ frc::SendableChooser<frc2::Command*> m_chooser;
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..fb3c19d
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,166 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/ADXRS450_Gyro.h>
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/drive/MecanumDrive.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/interfaces/Gyro.h>
+#include <frc/kinematics/MecanumDriveOdometry.h>
+#include <frc/kinematics/MecanumDriveWheelSpeeds.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+ DriveSubsystem();
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic() override;
+
+ // Subsystem methods go here.
+
+ /**
+ * Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1]
+ * and the linear speeds have no effect on the angular speed.
+ *
+ * @param xSpeed Speed of the robot in the x direction
+ * (forward/backwards).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
+ * @param fieldRelative Whether the provided x and y speeds are relative to
+ * the field.
+ */
+ void Drive(double xSpeed, double ySpeed, double rot, bool feildRelative);
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ void ResetEncoders();
+
+ /**
+ * Gets the front left drive encoder.
+ *
+ * @return the front left drive encoder
+ */
+ frc::Encoder& GetFrontLeftEncoder();
+
+ /**
+ * Gets the rear left drive encoder.
+ *
+ * @return the rear left drive encoder
+ */
+ frc::Encoder& GetRearLeftEncoder();
+
+ /**
+ * Gets the front right drive encoder.
+ *
+ * @return the front right drive encoder
+ */
+ frc::Encoder& GetFrontRightEncoder();
+
+ /**
+ * Gets the rear right drive encoder.
+ *
+ * @return the rear right drive encoder
+ */
+ frc::Encoder& GetRearRightEncoder();
+
+ /**
+ * Gets the wheel speeds.
+ *
+ * @return the current wheel speeds.
+ */
+ frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds();
+
+ /**
+ * Sets the drive SpeedControllers to a desired voltage.
+ */
+ void SetSpeedControllersVolts(units::volt_t frontLeftPower,
+ units::volt_t rearLeftPower,
+ units::volt_t frontRightPower,
+ units::volt_t rearRightPower);
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive
+ * more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ void SetMaxOutput(double maxOutput);
+
+ /**
+ * Returns the heading of the robot.
+ *
+ * @return the robot's heading in degrees, from 180 to 180
+ */
+ double GetHeading();
+
+ /**
+ * Zeroes the heading of the robot.
+ */
+ void ZeroHeading();
+
+ /**
+ * Returns the turn rate of the robot.
+ *
+ * @return The turn rate of the robot, in degrees per second
+ */
+ double GetTurnRate();
+
+ /**
+ * Returns the currently-estimated pose of the robot.
+ *
+ * @return The pose.
+ */
+ frc::Pose2d GetPose();
+
+ /**
+ * Resets the odometry to the specified pose.
+ *
+ * @param pose The pose to which to set the odometry.
+ */
+ void ResetOdometry(frc::Pose2d pose);
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+
+ // The motor controllers
+ frc::PWMVictorSPX m_frontLeft;
+ frc::PWMVictorSPX m_rearLeft;
+ frc::PWMVictorSPX m_frontRight;
+ frc::PWMVictorSPX m_rearRight;
+
+ // The robot's drive
+ frc::MecanumDrive m_drive{m_frontLeft, m_rearLeft, m_frontRight, m_rearRight};
+
+ // The front-left-side drive encoder
+ frc::Encoder m_frontLeftEncoder;
+
+ // The rear-left-side drive encoder
+ frc::Encoder m_rearLeftEncoder;
+
+ // The front-right--side drive encoder
+ frc::Encoder m_frontRightEncoder;
+
+ // The rear-right-side drive encoder
+ frc::Encoder m_rearRightEncoder;
+
+ // The gyro sensor
+ frc::ADXRS450_Gyro m_gyro;
+
+ // Odometry class for tracking robot pose
+ frc::MecanumDriveOdometry m_odometry;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
index c5d22d0..71da410 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
@@ -12,7 +12,7 @@
#include <frc/shuffleboard/Shuffleboard.h>
#include <frc/trajectory/Trajectory.h>
#include <frc/trajectory/TrajectoryGenerator.h>
-#include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
+#include <frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h>
#include <frc2/command/InstantCommand.h>
#include <frc2/command/RamseteCommand.h>
#include <frc2/command/SequentialCommandGroup.h>
@@ -44,11 +44,19 @@
}
frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Create a voltage constraint to ensure we don't accelerate too fast
+ frc::DifferentialDriveVoltageConstraint autoVoltageConstraint(
+ frc::SimpleMotorFeedforward<units::meters>(
+ DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
+ DriveConstants::kDriveKinematics, 10_V);
+
// Set up config for trajectory
frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
AutoConstants::kMaxAcceleration);
// Add kinematics to ensure max speed is actually obeyed
config.SetKinematics(DriveConstants::kDriveKinematics);
+ // Apply the voltage constraint
+ config.AddConstraint(autoVoltageConstraint);
// An example trajectory to follow. All units in meters.
auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
@@ -65,23 +73,17 @@
exampleTrajectory, [this]() { return m_drive.GetPose(); },
frc::RamseteController(AutoConstants::kRamseteB,
AutoConstants::kRamseteZeta),
- DriveConstants::ks, DriveConstants::kv, DriveConstants::ka,
+ frc::SimpleMotorFeedforward<units::meters>(
+ DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
DriveConstants::kDriveKinematics,
- [this] {
- return units::meters_per_second_t(m_drive.GetLeftEncoder().GetRate());
- },
- [this] {
- return units::meters_per_second_t(m_drive.GetRightEncoder().GetRate());
- },
+ [this] { return m_drive.GetWheelSpeeds(); },
frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
- [this](auto left, auto right) {
- m_drive.TankDrive(left / 12_V, right / 12_V);
- },
+ [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
{&m_drive});
// no auto
return new frc2::SequentialCommandGroup(
std::move(ramseteCommand),
- frc2::InstantCommand([this] { m_drive.TankDrive(0, 0); }, {}));
+ frc2::InstantCommand([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
}
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
index 3d5307f..be5f82a 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -7,8 +7,6 @@
#include "subsystems/DriveSubsystem.h"
-#include <units/units.h>
-
#include <frc/geometry/Rotation2d.h>
#include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
@@ -21,26 +19,28 @@
m_right2{kRightMotor2Port},
m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
- m_odometry{kDriveKinematics, frc::Pose2d()} {
+ m_odometry{frc::Rotation2d(units::degree_t(GetHeading()))} {
// Set the distance per pulse for the encoders
m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+
+ ResetEncoders();
}
void DriveSubsystem::Periodic() {
// Implementation of subsystem periodic method goes here.
m_odometry.Update(frc::Rotation2d(units::degree_t(GetHeading())),
- frc::DifferentialDriveWheelSpeeds{
- units::meters_per_second_t(m_leftEncoder.GetRate()),
- units::meters_per_second_t(m_rightEncoder.GetRate())});
+ units::meter_t(m_leftEncoder.GetDistance()),
+ units::meter_t(m_rightEncoder.GetDistance()));
}
void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
m_drive.ArcadeDrive(fwd, rot);
}
-void DriveSubsystem::TankDrive(double left, double right) {
- m_drive.TankDrive(left, right, false);
+void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
+ m_leftMotors.SetVoltage(left);
+ m_rightMotors.SetVoltage(-right);
}
void DriveSubsystem::ResetEncoders() {
@@ -49,7 +49,7 @@
}
double DriveSubsystem::GetAverageEncoderDistance() {
- return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
}
frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
@@ -61,15 +61,22 @@
}
double DriveSubsystem::GetHeading() {
- return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+ return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
}
double DriveSubsystem::GetTurnRate() {
- return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.);
+ return m_gyro.GetRate() * (kGyroReversed ? -1.0 : 1.0);
}
frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
+frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
+ return {units::meters_per_second_t(m_leftEncoder.GetRate()),
+ units::meters_per_second_t(m_rightEncoder.GetRate())};
+}
+
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
- m_odometry.ResetPosition(pose);
+ ResetEncoders();
+ m_odometry.ResetPosition(pose,
+ frc::Rotation2d(units::degree_t(GetHeading())));
}
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
index 801e479..22564c6 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
@@ -5,10 +5,10 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include <units/units.h>
-
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
+#include <units/units.h>
+#include <wpi/math>
#pragma once
@@ -22,50 +22,50 @@
*/
namespace DriveConstants {
-const int kLeftMotor1Port = 0;
-const int kLeftMotor2Port = 1;
-const int kRightMotor1Port = 2;
-const int kRightMotor2Port = 3;
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
-const int kLeftEncoderPorts[]{0, 1};
-const int kRightEncoderPorts[]{2, 3};
-const bool kLeftEncoderReversed = false;
-const bool kRightEncoderReversed = true;
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
-const auto kTrackwidth = .6_m;
-const frc::DifferentialDriveKinematics kDriveKinematics(kTrackwidth);
+constexpr auto kTrackwidth = 0.6_m;
+constexpr frc::DifferentialDriveKinematics kDriveKinematics(kTrackwidth);
-const int kEncoderCPR = 1024;
-const double kWheelDiameterInches = 6;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
- (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+ (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
-const bool kGyroReversed = true;
+constexpr bool kGyroReversed = true;
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
// These characterization values MUST be determined either experimentally or
// theoretically for *your* robot's drive. The RobotPy Characterization
// Toolsuite provides a convenient tool for obtaining these values for your
// robot.
-const auto ks = 1_V;
-const auto kv = .8 * 1_V * 1_s / 1_m;
-const auto ka = .15 * 1_V * 1_s * 1_s / 1_m;
+constexpr auto ks = 1_V;
+constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
+constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m;
// Example value only - as above, this must be tuned for your drive!
-const double kPDriveVel = .5;
+constexpr double kPDriveVel = 0.5;
} // namespace DriveConstants
namespace AutoConstants {
-const auto kMaxSpeed = 3_mps;
-const auto kMaxAcceleration = 3_mps_sq;
+constexpr auto kMaxSpeed = 3_mps;
+constexpr auto kMaxAcceleration = 3_mps_sq;
// Reasonable baseline values for a RAMSETE follower in units of meters and
// seconds
-const double kRamseteB = 2;
-const double kRamseteZeta = .7;
+constexpr double kRamseteB = 2;
+constexpr double kRamseteZeta = 0.7;
} // namespace AutoConstants
namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h
index cc91e0d..d6e4288 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h
@@ -41,7 +41,7 @@
// The robot's subsystems
DriveSubsystem m_drive;
- frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+ frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
{}};
frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
{}};
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
index 620cfd8..d8d1abd 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
@@ -15,6 +15,7 @@
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/DifferentialDriveOdometry.h>
#include <frc2/command/SubsystemBase.h>
+#include <units/units.h>
#include "Constants.h"
@@ -38,13 +39,12 @@
void ArcadeDrive(double fwd, double rot);
/**
- * Drives the robot using tank controls. Does not square inputs to enable
- * composition with external controllers.
+ * Controls each side of the drive directly with a voltage.
*
* @param left the commanded left output
* @param right the commanded right output
*/
- void TankDrive(double left, double right);
+ void TankDriveVolts(units::volt_t left, units::volt_t right);
/**
* Resets the drive encoders to currently read a position of 0.
@@ -102,6 +102,13 @@
frc::Pose2d GetPose();
/**
+ * Returns the current wheel speeds of the robot.
+ *
+ * @return The current wheel speeds.
+ */
+ frc::DifferentialDriveWheelSpeeds GetWheelSpeeds();
+
+ /**
* Resets the odometry to the specified pose.
*
* @param pose The pose to which to set the odometry.
diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h
index 0a5f832..9fb2281 100644
--- a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h
@@ -18,5 +18,5 @@
*/
namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
index 0a5f832..9fb2281 100644
--- a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
@@ -18,5 +18,5 @@
*/
namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
index 697984c..f7a11b8 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
@@ -26,7 +26,8 @@
// Limit the PID Controller's input range between -pi and pi and set the input
// to be continuous.
- m_turningPIDController.EnableContinuousInput(-wpi::math::pi, wpi::math::pi);
+ m_turningPIDController.EnableContinuousInput(-units::radian_t(wpi::math::pi),
+ units::radian_t(wpi::math::pi));
}
frc::SwerveModuleState SwerveModule::GetState() const {
@@ -41,10 +42,7 @@
// Calculate the turning motor output from the turning PID controller.
const auto turnOutput = m_turningPIDController.Calculate(
- units::meter_t(m_turningEncoder.Get()),
- // We have to convert to the meters type here because that's what
- // ProfiledPIDController wants.
- units::meter_t(state.angle.Radians().to<double>()));
+ units::radian_t(m_turningEncoder.Get()), state.angle.Radians());
// Set the motor outputs.
m_driveMotor.Set(driveOutput);
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
index 745581a..2de6e95 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
@@ -57,5 +57,5 @@
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
m_backRightLocation};
- frc::SwerveDriveOdometry<4> m_odometry{m_kinematics};
+ frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, GetAngle()};
};
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
index 0eaa69e..0dafa24 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
@@ -8,7 +8,7 @@
#pragma once
#include <frc/Encoder.h>
-#include <frc/Spark.h>
+#include <frc/PWMVictorSPX.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/kinematics/SwerveModuleState.h>
@@ -24,24 +24,19 @@
static constexpr double kWheelRadius = 0.0508;
static constexpr int kEncoderResolution = 4096;
- // We have to use meters here instead of radians because of the fact that
- // ProfiledPIDController's constraints only take in meters per second and
- // meters per second squared.
+ static constexpr auto kModuleMaxAngularVelocity =
+ wpi::math::pi * 1_rad_per_s; // radians per second
+ static constexpr auto kModuleMaxAngularAcceleration =
+ wpi::math::pi * 2_rad_per_s / 1_s; // radians per second^2
- static constexpr units::meters_per_second_t kModuleMaxAngularVelocity =
- units::meters_per_second_t(wpi::math::pi); // radians per second
- static constexpr units::meters_per_second_squared_t
- kModuleMaxAngularAcceleration = units::meters_per_second_squared_t(
- wpi::math::pi * 2.0); // radians per second squared
-
- frc::Spark m_driveMotor;
- frc::Spark m_turningMotor;
+ frc::PWMVictorSPX m_driveMotor;
+ frc::PWMVictorSPX m_turningMotor;
frc::Encoder m_driveEncoder{0, 1};
- frc::Encoder m_turningEncoder{0, 1};
+ frc::Encoder m_turningEncoder{2, 3};
frc2::PIDController m_drivePIDController{1.0, 0, 0};
- frc::ProfiledPIDController m_turningPIDController{
+ frc::ProfiledPIDController<units::radians> m_turningPIDController{
1.0,
0.0,
0.0,
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..44efba5
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp
@@ -0,0 +1,91 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/controller/PIDController.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc/trajectory/Trajectory.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/SwerveControllerCommand.h>
+#include <frc2/command/button/JoystickButton.h>
+#include <units/units.h>
+
+#include "Constants.h"
+#include "subsystems/DriveSubsystem.h"
+
+using namespace DriveConstants;
+
+RobotContainer::RobotContainer() {
+ // Initialize all of your commands and subsystems here
+
+ // Configure the button bindings
+ ConfigureButtonBindings();
+
+ // Set up default drive command
+ m_drive.SetDefaultCommand(frc2::RunCommand(
+ [this] {
+ m_drive.Drive(units::meters_per_second_t(
+ m_driverController.GetY(frc::GenericHID::kLeftHand)),
+ units::meters_per_second_t(
+ m_driverController.GetY(frc::GenericHID::kRightHand)),
+ units::radians_per_second_t(
+ m_driverController.GetX(frc::GenericHID::kLeftHand)),
+ false);
+ },
+ {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+ // Set up config for trajectory
+ frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
+ AutoConstants::kMaxAcceleration);
+ // Add kinematics to ensure max speed is actually obeyed
+ config.SetKinematics(m_drive.kDriveKinematics);
+
+ // An example trajectory to follow. All units in meters.
+ auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ // Start at the origin facing the +X direction
+ frc::Pose2d(0_m, 0_m, frc::Rotation2d(0_deg)),
+ // Pass through these two interior waypoints, making an 's' curve path
+ {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
+ // End 3 meters straight ahead of where we started, facing forward
+ frc::Pose2d(3_m, 0_m, frc::Rotation2d(0_deg)),
+ // Pass the config
+ config);
+
+ frc2::SwerveControllerCommand<4> swerveControllerCommand(
+ exampleTrajectory, [this]() { return m_drive.GetPose(); },
+
+ m_drive.kDriveKinematics,
+
+ frc2::PIDController(AutoConstants::kPXController, 0, 0),
+ frc2::PIDController(AutoConstants::kPYController, 0, 0),
+ frc::ProfiledPIDController<units::radians>(
+ AutoConstants::kPThetaController, 0, 0,
+ AutoConstants::kThetaControllerConstraints),
+
+ [this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); },
+
+ {&m_drive});
+
+ // no auto
+ return new frc2::SequentialCommandGroup(
+ std::move(swerveControllerCommand), std::move(swerveControllerCommand),
+ frc2::InstantCommand(
+ [this]() {
+ m_drive.Drive(units::meters_per_second_t(0),
+ units::meters_per_second_t(0),
+ units::radians_per_second_t(0), false);
+ },
+ {}));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..f5d05ce
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+#include <frc/geometry/Rotation2d.h>
+#include <units/units.h>
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+ : m_frontLeft{kFrontLeftDriveMotorPort,
+ kFrontLeftTurningMotorPort,
+ kFrontLeftDriveEncoderPorts,
+ kFrontLeftTurningEncoderPorts,
+ kFrontLeftDriveEncoderReversed,
+ kFrontLeftTurningEncoderReversed},
+
+ m_rearLeft{
+ kRearLeftDriveMotorPort, kRearLeftTurningMotorPort,
+ kRearLeftDriveEncoderPorts, kRearLeftTurningEncoderPorts,
+ kRearLeftDriveEncoderReversed, kRearLeftTurningEncoderReversed},
+
+ m_frontRight{
+ kFrontRightDriveMotorPort, kFrontRightTurningMotorPort,
+ kFrontRightDriveEncoderPorts, kFrontRightTurningEncoderPorts,
+ kFrontRightDriveEncoderReversed, kFrontRightTurningEncoderReversed},
+
+ m_rearRight{
+ kRearRightDriveMotorPort, kRearRightTurningMotorPort,
+ kRearRightDriveEncoderPorts, kRearRightTurningEncoderPorts,
+ kRearRightDriveEncoderReversed, kRearRightTurningEncoderReversed},
+
+ m_odometry{kDriveKinematics,
+ frc::Rotation2d(units::degree_t(GetHeading())),
+ frc::Pose2d()} {}
+
+void DriveSubsystem::Periodic() {
+ // Implementation of subsystem periodic method goes here.
+ m_odometry.Update(frc::Rotation2d(units::degree_t(GetHeading())),
+ m_frontLeft.GetState(), m_rearLeft.GetState(),
+ m_frontRight.GetState(), m_rearRight.GetState());
+}
+
+void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
+ units::meters_per_second_t ySpeed,
+ units::radians_per_second_t rot,
+ bool fieldRelative) {
+ auto states = kDriveKinematics.ToSwerveModuleStates(
+ fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+ xSpeed, ySpeed, rot,
+ frc::Rotation2d(units::degree_t(GetHeading())))
+ : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
+
+ kDriveKinematics.NormalizeWheelSpeeds(&states, AutoConstants::kMaxSpeed);
+
+ auto [fl, fr, bl, br] = states;
+
+ m_frontLeft.SetDesiredState(fl);
+ m_frontRight.SetDesiredState(fr);
+ m_rearLeft.SetDesiredState(bl);
+ m_rearRight.SetDesiredState(br);
+}
+
+void DriveSubsystem::SetModuleStates(
+ std::array<frc::SwerveModuleState, 4> desiredStates) {
+ kDriveKinematics.NormalizeWheelSpeeds(&desiredStates,
+ AutoConstants::kMaxSpeed);
+ m_frontLeft.SetDesiredState(desiredStates[0]);
+ m_rearLeft.SetDesiredState(desiredStates[1]);
+ m_frontRight.SetDesiredState(desiredStates[2]);
+ m_rearRight.SetDesiredState(desiredStates[3]);
+}
+
+void DriveSubsystem::ResetEncoders() {
+ m_frontLeft.ResetEncoders();
+ m_rearLeft.ResetEncoders();
+ m_frontRight.ResetEncoders();
+ m_rearRight.ResetEncoders();
+}
+
+double DriveSubsystem::GetHeading() {
+ return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+}
+
+void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); }
+
+double DriveSubsystem::GetTurnRate() {
+ return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.);
+}
+
+frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
+
+void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
+ m_odometry.ResetPosition(pose,
+ frc::Rotation2d(units::degree_t(GetHeading())));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp
new file mode 100644
index 0000000..22b3f0d
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/SwerveModule.h"
+
+#include <frc/geometry/Rotation2d.h>
+#include <wpi/math>
+
+#include "Constants.h"
+
+SwerveModule::SwerveModule(int driveMotorChannel, int turningMotorChannel,
+ const int driveEncoderPorts[],
+ const int turningEncoderPorts[],
+ bool driveEncoderReversed,
+ bool turningEncoderReversed)
+ : m_driveMotor(driveMotorChannel),
+ m_turningMotor(turningMotorChannel),
+ m_driveEncoder(driveEncoderPorts[0], driveEncoderPorts[1]),
+ m_turningEncoder(turningEncoderPorts[0], turningEncoderPorts[1]),
+ m_reverseDriveEncoder(driveEncoderReversed),
+ m_reverseTurningEncoder(turningEncoderReversed) {
+ // Set the distance per pulse for the drive encoder. We can simply use the
+ // distance traveled for one rotation of the wheel divided by the encoder
+ // resolution.
+ m_driveEncoder.SetDistancePerPulse(
+ ModuleConstants::kDriveEncoderDistancePerPulse);
+
+ // Set the distance (in this case, angle) per pulse for the turning encoder.
+ // This is the the angle through an entire rotation (2 * wpi::math::pi)
+ // divided by the encoder resolution.
+ m_turningEncoder.SetDistancePerPulse(
+ ModuleConstants::kTurningEncoderDistancePerPulse);
+
+ // Limit the PID Controller's input range between -pi and pi and set the input
+ // to be continuous.
+ m_turningPIDController.EnableContinuousInput(units::radian_t(-wpi::math::pi),
+ units::radian_t(wpi::math::pi));
+}
+
+frc::SwerveModuleState SwerveModule::GetState() {
+ return {units::meters_per_second_t{m_driveEncoder.GetRate()},
+ frc::Rotation2d(units::radian_t(m_turningEncoder.Get()))};
+}
+
+void SwerveModule::SetDesiredState(frc::SwerveModuleState& state) {
+ // Calculate the drive output from the drive PID controller.
+ const auto driveOutput = m_drivePIDController.Calculate(
+ m_driveEncoder.GetRate(), state.speed.to<double>());
+
+ // Calculate the turning motor output from the turning PID controller.
+ auto turnOutput = m_turningPIDController.Calculate(
+ units::radian_t(m_turningEncoder.Get()), state.angle.Radians());
+
+ // Set the motor outputs.
+ m_driveMotor.Set(driveOutput);
+ m_turningMotor.Set(turnOutput);
+}
+
+void SwerveModule::ResetEncoders() {
+ m_driveEncoder.Reset();
+ m_turningEncoder.Reset();
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
new file mode 100644
index 0000000..ed1f880
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <units/units.h>
+#include <wpi/math>
+
+#pragma once
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants. This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+constexpr int kFrontLeftDriveMotorPort = 0;
+constexpr int kRearLeftDriveMotorPort = 2;
+constexpr int kFrontRightDriveMotorPort = 4;
+constexpr int kRearRightDriveMotorPort = 6;
+
+constexpr int kFrontLeftTurningMotorPort = 1;
+constexpr int kRearLeftTurningMotorPort = 3;
+constexpr int kFrontRightTurningMotorPort = 5;
+constexpr int kRearRightTurningMotorPort = 7;
+
+constexpr int kFrontLeftTurningEncoderPorts[2]{0, 1};
+constexpr int kRearLeftTurningEncoderPorts[2]{2, 3};
+constexpr int kFrontRightTurningEncoderPorts[2]{4, 5};
+constexpr int kRearRightTurningEncoderPorts[2]{5, 6};
+
+constexpr bool kFrontLeftTurningEncoderReversed = false;
+constexpr bool kRearLeftTurningEncoderReversed = true;
+constexpr bool kFrontRightTurningEncoderReversed = false;
+constexpr bool kRearRightTurningEncoderReversed = true;
+
+constexpr int kFrontLeftDriveEncoderPorts[2]{0, 1};
+constexpr int kRearLeftDriveEncoderPorts[2]{2, 3};
+constexpr int kFrontRightDriveEncoderPorts[2]{4, 5};
+constexpr int kRearRightDriveEncoderPorts[2]{5, 6};
+
+constexpr bool kFrontLeftDriveEncoderReversed = false;
+constexpr bool kRearLeftDriveEncoderReversed = true;
+constexpr bool kFrontRightDriveEncoderReversed = false;
+constexpr bool kRearRightDriveEncoderReversed = true;
+
+constexpr bool kGyroReversed = false;
+
+// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+// These characterization values MUST be determined either experimentally or
+// theoretically for *your* robot's drive. The RobotPy Characterization
+// Toolsuite provides a convenient tool for obtaining these values for your
+// robot.
+constexpr auto ks = 1_V;
+constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
+constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m;
+
+// Example value only - as above, this must be tuned for your drive!
+constexpr double kPFrontLeftVel = 0.5;
+constexpr double kPRearLeftVel = 0.5;
+constexpr double kPFrontRightVel = 0.5;
+constexpr double kPRearRightVel = 0.5;
+} // namespace DriveConstants
+
+namespace ModuleConstants {
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterMeters = .15;
+constexpr double kDriveEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterMeters * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+
+constexpr double kTurningEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (wpi::math::pi * 2) / static_cast<double>(kEncoderCPR);
+
+constexpr double kPModuleTurningController = 1;
+constexpr double kPModuleDriveController = 1;
+} // namespace ModuleConstants
+
+namespace AutoConstants {
+using radians_per_second_squared_t =
+ units::compound_unit<units::radians,
+ units::inverse<units::squared<units::second>>>;
+
+constexpr auto kMaxSpeed = units::meters_per_second_t(3);
+constexpr auto kMaxAcceleration = units::meters_per_second_squared_t(3);
+constexpr auto kMaxAngularSpeed = units::radians_per_second_t(3.142);
+constexpr auto kMaxAngularAcceleration =
+ units::unit_t<radians_per_second_squared_t>(3.142);
+
+constexpr double kPXController = 0.5;
+constexpr double kPYController = 0.5;
+constexpr double kPThetaController = 0.5;
+
+//
+
+constexpr frc::TrapezoidProfile<units::radians>::Constraints
+ kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration};
+
+} // namespace AutoConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+} // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h
similarity index 60%
copy from wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
copy to wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h
index 2c70b8f..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,15 +7,17 @@
#pragma once
-#include <string>
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
-#include <frc/IterativeRobot.h>
-#include <frc/smartdashboard/SendableChooser.h>
+#include "RobotContainer.h"
-class Robot : public frc::IterativeRobot {
+class Robot : public frc::TimedRobot {
public:
void RobotInit() override;
void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
void AutonomousInit() override;
void AutonomousPeriodic() override;
void TeleopInit() override;
@@ -23,8 +25,9 @@
void TestPeriodic() override;
private:
- frc::SendableChooser<std::string> m_chooser;
- const std::string kAutoNameDefault = "Default";
- const std::string kAutoNameCustom = "My Auto";
- std::string m_autoSelected;
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc2::Command* m_autonomousCommand = nullptr;
+
+ RobotContainer m_container;
};
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h
new file mode 100644
index 0000000..8b36617
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/PIDCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+
+#include "Constants.h"
+#include "subsystems/DriveSubsystem.h"
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+ RobotContainer();
+
+ frc2::Command* GetAutonomousCommand();
+
+ private:
+ // The driver's controller
+ frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+ // The robot's subsystems and commands are defined here...
+
+ // The robot's subsystems
+ DriveSubsystem m_drive;
+
+ // The chooser for the autonomous routines
+ frc::SendableChooser<frc2::Command*> m_chooser;
+
+ void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..db4623f
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/ADXRS450_Gyro.h>
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/drive/MecanumDrive.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/interfaces/Gyro.h>
+#include <frc/kinematics/ChassisSpeeds.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/kinematics/SwerveDriveOdometry.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+#include "SwerveModule.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+ DriveSubsystem();
+
+ /**
+ * Will be called periodically whenever the CommandScheduler runs.
+ */
+ void Periodic() override;
+
+ // Subsystem methods go here.
+
+ /**
+ * Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1]
+ * and the linear speeds have no effect on the angular speed.
+ *
+ * @param xSpeed Speed of the robot in the x direction
+ * (forward/backwards).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
+ * @param fieldRelative Whether the provided x and y speeds are relative to
+ * the field.
+ */
+ void Drive(units::meters_per_second_t xSpeed,
+ units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
+ bool feildRelative);
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ void ResetEncoders();
+
+ /**
+ * Sets the drive SpeedControllers to a power from -1 to 1.
+ */
+ void SetModuleStates(std::array<frc::SwerveModuleState, 4> desiredStates);
+
+ /**
+ * Returns the heading of the robot.
+ *
+ * @return the robot's heading in degrees, from 180 to 180
+ */
+ double GetHeading();
+
+ /**
+ * Zeroes the heading of the robot.
+ */
+ void ZeroHeading();
+
+ /**
+ * Returns the turn rate of the robot.
+ *
+ * @return The turn rate of the robot, in degrees per second
+ */
+ double GetTurnRate();
+
+ /**
+ * Returns the currently-estimated pose of the robot.
+ *
+ * @return The pose.
+ */
+ frc::Pose2d GetPose();
+
+ /**
+ * Resets the odometry to the specified pose.
+ *
+ * @param pose The pose to which to set the odometry.
+ */
+ void ResetOdometry(frc::Pose2d pose);
+
+ units::meter_t kTrackWidth =
+ .5_m; // Distance between centers of right and left wheels on robot
+ units::meter_t kWheelBase =
+ .7_m; // Distance between centers of front and back wheels on robot
+
+ frc::SwerveDriveKinematics<4> kDriveKinematics{
+ frc::Translation2d(kWheelBase / 2, kTrackWidth / 2),
+ frc::Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+ frc::Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+ frc::Translation2d(-kWheelBase / 2, -kTrackWidth / 2)};
+
+ private:
+ // Components (e.g. motor controllers and sensors) should generally be
+ // declared private and exposed only through public methods.
+
+ SwerveModule m_frontLeft;
+ SwerveModule m_rearLeft;
+ SwerveModule m_frontRight;
+ SwerveModule m_rearRight;
+
+ // The gyro sensor
+ frc::ADXRS450_Gyro m_gyro;
+
+ // Odometry class for tracking robot pose
+ // 4 defines the number of modules
+ frc::SwerveDriveOdometry<4> m_odometry;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h
new file mode 100644
index 0000000..f8d816a
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/Spark.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <wpi/math>
+
+#include "Constants.h"
+
+class SwerveModule {
+ using radians_per_second_squared_t =
+ units::compound_unit<units::radians,
+ units::inverse<units::squared<units::second>>>;
+
+ public:
+ SwerveModule(int driveMotorChannel, int turningMotorChannel,
+ const int driveEncoderPorts[2], const int turningEncoderPorts[2],
+ bool driveEncoderReversed, bool turningEncoderReversed);
+
+ frc::SwerveModuleState GetState();
+
+ void SetDesiredState(frc::SwerveModuleState& state);
+
+ void ResetEncoders();
+
+ private:
+ // We have to use meters here instead of radians due to the fact that
+ // ProfiledPIDController's constraints only take in meters per second and
+ // meters per second squared.
+
+ static constexpr units::radians_per_second_t kModuleMaxAngularVelocity =
+ units::radians_per_second_t(wpi::math::pi); // radians per second
+ static constexpr units::unit_t<radians_per_second_squared_t>
+ kModuleMaxAngularAcceleration =
+ units::unit_t<radians_per_second_squared_t>(
+ wpi::math::pi * 2.0); // radians per second squared
+
+ frc::Spark m_driveMotor;
+ frc::Spark m_turningMotor;
+
+ frc::Encoder m_driveEncoder;
+ frc::Encoder m_turningEncoder;
+
+ bool m_reverseDriveEncoder;
+ bool m_reverseTurningEncoder;
+
+ frc2::PIDController m_drivePIDController{
+ ModuleConstants::kPModuleDriveController, 0, 0};
+ frc::ProfiledPIDController<units::radians> m_turningPIDController{
+ ModuleConstants::kPModuleTurningController,
+ 0.0,
+ 0.0,
+ {kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp
new file mode 100644
index 0000000..3ca31ed
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/GenericHID.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/drive/DifferentialDrive.h>
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class.
+ * Runs the motors with tank steering and an Xbox controller.
+ */
+class Robot : public frc::TimedRobot {
+ frc::PWMVictorSPX m_leftMotor{0};
+ frc::PWMVictorSPX m_rightMotor{1};
+ frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
+ frc::XboxController m_driverController{0};
+
+ public:
+ void TeleopPeriodic() {
+ // Drive with tank style
+ m_robotDrive.TankDrive(
+ m_driverController.GetY(frc::GenericHID::JoystickHand::kLeftHand),
+ m_driverController.GetY(frc::GenericHID::JoystickHand::kRightHand));
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
index 2cc2131..9f9c01d 100644
--- a/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
@@ -1,11 +1,12 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <frc/AnalogInput.h>
+#include <frc/MedianFilter.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/drive/DifferentialDrive.h>
@@ -22,7 +23,10 @@
*/
void TeleopPeriodic() override {
// Sensor returns a value from 0-4095 that is scaled to inches
- double currentDistance = m_ultrasonic.GetValue() * kValueToInches;
+ // returned value is filtered with a rolling median filter, since
+ // ultrasonics tend to be quite noisy and susceptible to sudden outliers
+ double currentDistance =
+ m_filter.Calculate(m_ultrasonic.GetVoltage()) * kValueToInches;
// Convert distance error to a motor speed
double currentSpeed = (kHoldDistance - currentDistance) * kP;
// Drive robot
@@ -43,6 +47,9 @@
static constexpr int kRightMotorPort = 1;
static constexpr int kUltrasonicPort = 0;
+ // median filter to discard outliers; filters over 10 samples
+ frc::MedianFilter<double> m_filter{10};
+
frc::AnalogInput m_ultrasonic{kUltrasonicPort};
frc::PWMVictorSPX m_left{kLeftMotorPort};
diff --git a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
index 745fc10..47bd62d 100644
--- a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
@@ -6,6 +6,7 @@
/*----------------------------------------------------------------------------*/
#include <frc/AnalogInput.h>
+#include <frc/MedianFilter.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/controller/PIDController.h>
@@ -27,7 +28,8 @@
}
void TeleopPeriodic() override {
- double output = m_pidController.Calculate(m_ultrasonic.GetAverageVoltage());
+ double output =
+ m_pidController.Calculate(m_filter.Calculate(m_ultrasonic.GetValue()));
m_robotDrive.ArcadeDrive(output, 0);
}
@@ -51,6 +53,9 @@
static constexpr int kRightMotorPort = 1;
static constexpr int kUltrasonicPort = 0;
+ // median filter to discard outliers; filters over 5 samples
+ frc::MedianFilter<double> m_filter{5};
+
frc::AnalogInput m_ultrasonic{kUltrasonicPort};
frc::PWMVictorSPX m_left{kLeftMotorPort};
diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json
index 864ea1d..4aa9f0e 100644
--- a/wpilibcExamples/src/main/cpp/examples/examples.json
+++ b/wpilibcExamples/src/main/cpp/examples/examples.json
@@ -9,7 +9,8 @@
"Complete List"
],
"foldername": "MotorControl",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Motor Control With Encoder",
@@ -23,7 +24,8 @@
"Complete List"
],
"foldername": "MotorControlEncoder",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Relay",
@@ -34,7 +36,8 @@
"Complete List"
],
"foldername": "Relay",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "PDP CAN Monitoring",
@@ -45,7 +48,8 @@
"Sensors"
],
"foldername": "CANPDP",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Solenoids",
@@ -57,7 +61,8 @@
"Complete List"
],
"foldername": "Solenoid",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Encoder",
@@ -68,7 +73,8 @@
"Sensors"
],
"foldername": "Encoder",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Arcade Drive",
@@ -80,7 +86,8 @@
"Complete List"
],
"foldername": "ArcadeDrive",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Mecanum Drive",
@@ -92,7 +99,8 @@
"Complete List"
],
"foldername": "MecanumDrive",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Ultrasonic",
@@ -104,7 +112,8 @@
"Analog"
],
"foldername": "Ultrasonic",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "UltrasonicPID",
@@ -116,7 +125,8 @@
"Analog"
],
"foldername": "UltrasonicPID",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Gyro",
@@ -129,7 +139,8 @@
"Joystick"
],
"foldername": "Gyro",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Gyro Mecanum",
@@ -142,7 +153,8 @@
"Joysitck"
],
"foldername": "GyroMecanum",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "HID Rumble",
@@ -151,7 +163,8 @@
"Joystick"
],
"foldername": "HidRumble",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "PotentiometerPID",
@@ -164,7 +177,8 @@
"Analog"
],
"foldername": "PotentiometerPID",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Elevator with trapezoid profiled PID",
@@ -176,7 +190,8 @@
"Joystick"
],
"foldername": "ElevatorTrapezoidProfile",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Elevator with profiled PID controller",
@@ -188,7 +203,8 @@
"Joystick"
],
"foldername": "ElevatorProfiledPID",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Getting Started",
@@ -198,7 +214,8 @@
"Complete List"
],
"foldername": "GettingStarted",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Simple Vision",
@@ -208,7 +225,8 @@
"Complete List"
],
"foldername": "QuickVision",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Intermediate Vision",
@@ -218,7 +236,8 @@
"Complete List"
],
"foldername": "IntermediateVision",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Axis Camera Sample",
@@ -228,7 +247,8 @@
"Complete List"
],
"foldername": "AxisCameraSample",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "GearsBot",
@@ -238,7 +258,8 @@
"Complete List"
],
"foldername": "GearsBot",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "PacGoat",
@@ -248,7 +269,8 @@
"Complete List"
],
"foldername": "PacGoat",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "HAL",
@@ -257,7 +279,8 @@
"HAL"
],
"foldername": "HAL",
- "gradlebase": "c"
+ "gradlebase": "c",
+ "commandversion": 1
},
{
"name": "ShuffleBoard",
@@ -266,7 +289,8 @@
"ShuffleBoard"
],
"foldername": "ShuffleBoard",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "'Traditional' Hatchbot",
@@ -276,7 +300,8 @@
"Command-based"
],
"foldername": "HatchbotTraditional",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "'Inlined' Hatchbot",
@@ -287,7 +312,8 @@
"Lambdas"
],
"foldername": "HatchbotInlined",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "Select Command Example",
@@ -296,7 +322,8 @@
"Command-based"
],
"foldername": "SelectCommand",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "Scheduler Event Logging",
@@ -306,7 +333,8 @@
"Shuffleboard"
],
"foldername": "SchedulerEventLogging",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "Frisbeebot",
@@ -316,7 +344,8 @@
"PID"
],
"foldername": "Frisbeebot",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "Gyro Drive Commands",
@@ -327,7 +356,8 @@
"Gyro"
],
"foldername": "GyroDriveCommands",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "SwerveBot",
@@ -336,7 +366,8 @@
"SwerveBot"
],
"foldername": "SwerveBot",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "MecanumBot",
@@ -345,7 +376,8 @@
"MecanumBot"
],
"foldername": "MecanumBot",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
"name": "DifferentialDriveBot",
@@ -354,10 +386,11 @@
"DifferentialDriveBot"
],
"foldername": "DifferentialDriveBot",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
},
{
- "name:": "RamseteCommand",
+ "name": "RamseteCommand",
"description": "An example command-based robot demonstrating the use of a RamseteCommand to follow a pregenerated trajectory.",
"tags": [
"RamseteCommand",
@@ -367,6 +400,137 @@
"Path following"
],
"foldername": "RamseteCommand",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "Arcade Drive Xbox Controller",
+ "description": "An example program which demonstrates the use of Arcade Drive with the DifferentialDrive class and an Xbox Controller.",
+ "tags": [
+ "Getting Started with C++",
+ "Robot and Motor",
+ "XboxController",
+ "Complete List"
+ ],
+ "foldername": "ArcadeDriveXboxController",
+ "gradlebase": "cpp",
+ "commandversion": 1
+ },
+ {
+ "name": "Tank Drive Xbox Controller",
+ "description": "An example program which demonstrates the use of Tank Drive with the DifferentialDrive class and an Xbox Controller.",
+ "tags": [
+ "Getting Started with C++",
+ "Robot and Motor",
+ "XboxController",
+ "Complete List"
+ ],
+ "foldername": "TankDriveXboxController",
+ "gradlebase": "cpp",
+ "commandversion": 1
+ },
+ {
+ "name": "Duty Cycle Encoder",
+ "description": "Demonstrates the use of the Duty Cycle Encoder class",
+ "tags": [
+ "Getting Started with C++"
+ ],
+ "foldername": "DutyCycleEncoder",
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "Duty Cycle Input",
+ "description": "Demonstrates the use of the Duty Cycle class",
+ "tags": [
+ "Getting Started with C++"
+ ],
+ "foldername": "DutyCycleInput",
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "Addressable LED",
+ "description": "Demonstrates the use of the Addressable LED class",
+ "tags": [
+ "Getting Started with C++"
+ ],
+ "foldername": "AddressableLED",
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "DMA",
+ "description": "Demonstrates the use of the DMA class",
+ "tags": [
+ "Advanced C++"
+ ],
+ "foldername": "DMA",
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "MecanumControllerCommand",
+ "description": "An example command-based robot demonstrating the use of a MecanumControllerCommand to follow a pregenerated trajectory.",
+ "tags": [
+ "MecanumControllerCommand",
+ "Mecanum",
+ "PID",
+ "Trajectory",
+ "Path following"
+ ],
+ "foldername": "MecanumControllerCommand",
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "SwerveControllerCommand",
+ "description": "An example command-based robot demonstrating the use of a SwerveControllerCommand to follow a pregenerated trajectory.",
+ "tags": [
+ "SwerveControllerCommand",
+ "Swerve",
+ "PID",
+ "Trajectory",
+ "Path following"
+ ],
+ "foldername": "SwerveControllerCommand",
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "ArmBot",
+ "description": "An example command-based robot demonstrating the use of a ProfiledPIDSubsystem to control an arm.",
+ "tags": [
+ "ArmBot",
+ "PID",
+ "Motion Profile"
+ ],
+ "foldername": "ArmBot",
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "ArmBotOffboard",
+ "description": "An example command-based robot demonstrating the use of a TrapezoidProfileSubsystem to control an arm with an offboard PID.",
+ "tags": [
+ "ArmBotOffboard",
+ "PID",
+ "Motion Profile"
+ ],
+ "foldername": "ArmBotOffboard",
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "DriveDistanceOffboard",
+ "description": "An example command-based robot demonstrating the use of a TrapezoidProfileCommand to drive a robot a set distance with offboard PID on the drive.",
+ "tags": [
+ "DriveDistance",
+ "PID",
+ "Motion Profile"
+ ],
+ "foldername": "DriveDistanceOffboard",
+ "gradlebase": "cpp",
+ "commandversion": 2
}
]
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/iterative/cpp/Robot.cpp
deleted file mode 100644
index 942fc9a..0000000
--- a/wpilibcExamples/src/main/cpp/templates/iterative/cpp/Robot.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "Robot.h"
-
-#include <iostream>
-
-#include <frc/smartdashboard/SmartDashboard.h>
-
-void Robot::RobotInit() {
- m_chooser.SetDefaultOption(kAutoNameDefault, kAutoNameDefault);
- m_chooser.AddOption(kAutoNameCustom, kAutoNameCustom);
- frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
-}
-
-/**
- * This function is called every robot packet, no matter the mode. Use
- * this for items like diagnostics that you want ran during disabled,
- * autonomous, teleoperated and test.
- *
- * <p> This runs after the mode specific periodic functions, but before
- * LiveWindow and SmartDashboard integrated updating.
- */
-void Robot::RobotPeriodic() {}
-
-/**
- * This autonomous (along with the chooser code above) shows how to select
- * between different autonomous modes using the dashboard. The sendable chooser
- * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
- * remove all of the chooser code and uncomment the GetString line to get the
- * auto name from the text box below the Gyro.
- *
- * You can add additional auto modes by adding additional comparisons to the
- * if-else structure below with additional strings. If using the SendableChooser
- * make sure to add them to the chooser code above as well.
- */
-void Robot::AutonomousInit() {
- m_autoSelected = m_chooser.GetSelected();
- // m_autoSelected = SmartDashboard::GetString(
- // "Auto Selector", kAutoNameDefault);
- std::cout << "Auto selected: " << m_autoSelected << std::endl;
-
- if (m_autoSelected == kAutoNameCustom) {
- // Custom Auto goes here
- } else {
- // Default Auto goes here
- }
-}
-
-void Robot::AutonomousPeriodic() {
- if (m_autoSelected == kAutoNameCustom) {
- // Custom Auto goes here
- } else {
- // Default Auto goes here
- }
-}
-
-void Robot::TeleopInit() {}
-
-void Robot::TeleopPeriodic() {}
-
-void Robot::TestPeriodic() {}
-
-#ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
-#endif
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/OI.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/OI.cpp
new file mode 100644
index 0000000..2a9ef50
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/OI.cpp
@@ -0,0 +1,12 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be 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"
+
+OI::OI() {
+ // Process operator interface input here.
+}
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/Robot.cpp
new file mode 100644
index 0000000..b2a8884
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/Robot.cpp
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/commands/Scheduler.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+ExampleSubsystem Robot::m_subsystem;
+OI Robot::m_oi;
+
+void Robot::RobotInit() {
+ m_chooser.SetDefaultOption("Default Auto", &m_defaultAuto);
+ m_chooser.AddOption("My Auto", &m_myAuto);
+ frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want ran during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() {}
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+
+/**
+ * This autonomous (along with the chooser code above) shows how to select
+ * between different autonomous modes using the dashboard. The sendable chooser
+ * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
+ * remove all of the chooser code and uncomment the GetString code to get the
+ * auto name from the text box below the Gyro.
+ *
+ * You can add additional auto modes by adding additional commands to the
+ * chooser code above (like the commented example) or additional comparisons to
+ * the if-else structure below with additional strings & commands.
+ */
+void Robot::AutonomousInit() {
+ // std::string autoSelected = frc::SmartDashboard::GetString(
+ // "Auto Selector", "Default");
+ // if (autoSelected == "My Auto") {
+ // m_autonomousCommand = &m_myAuto;
+ // } else {
+ // m_autonomousCommand = &m_defaultAuto;
+ // }
+
+ m_autonomousCommand = m_chooser.GetSelected();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Start();
+ }
+}
+
+void Robot::AutonomousPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+void Robot::TeleopPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/ExampleCommand.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/ExampleCommand.cpp
new file mode 100644
index 0000000..a5596ef
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/ExampleCommand.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/ExampleCommand.h"
+
+#include "Robot.h"
+
+ExampleCommand::ExampleCommand() {
+ // Use Requires() here to declare subsystem dependencies
+ Requires(&Robot::m_subsystem);
+}
+
+// Called just before this Command runs the first time
+void ExampleCommand::Initialize() {}
+
+// Called repeatedly when this Command is scheduled to run
+void ExampleCommand::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool ExampleCommand::IsFinished() { return false; }
+
+// Called once after isFinished returns true
+void ExampleCommand::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void ExampleCommand::Interrupted() {}
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/MyAutoCommand.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/MyAutoCommand.cpp
new file mode 100644
index 0000000..1389447
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/MyAutoCommand.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/MyAutoCommand.h"
+
+#include "Robot.h"
+
+MyAutoCommand::MyAutoCommand() {
+ // Use Requires() here to declare subsystem dependencies
+ Requires(&Robot::m_subsystem);
+}
+
+// Called just before this Command runs the first time
+void MyAutoCommand::Initialize() {}
+
+// Called repeatedly when this Command is scheduled to run
+void MyAutoCommand::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool MyAutoCommand::IsFinished() { return false; }
+
+// Called once after isFinished returns true
+void MyAutoCommand::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void MyAutoCommand::Interrupted() {}
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/subsystems/ExampleSubsystem.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/subsystems/ExampleSubsystem.cpp
new file mode 100644
index 0000000..ed60550
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/subsystems/ExampleSubsystem.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/ExampleSubsystem.h"
+
+#include "RobotMap.h"
+
+ExampleSubsystem::ExampleSubsystem() : frc::Subsystem("ExampleSubsystem") {}
+
+void ExampleSubsystem::InitDefaultCommand() {
+ // Set the default command for a subsystem here.
+ // SetDefaultCommand(new MySpecialCommand());
+}
+
+// Put methods for controlling this subsystem
+// here. Call these from Commands.
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/OI.h b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/OI.h
new file mode 100644
index 0000000..77007bb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/OI.h
@@ -0,0 +1,13 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+class OI {
+ public:
+ OI();
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/Robot.h
new file mode 100644
index 0000000..94c2314
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/Robot.h
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc/commands/Command.h>
+#include <frc/smartdashboard/SendableChooser.h>
+
+#include "OI.h"
+#include "commands/ExampleCommand.h"
+#include "commands/MyAutoCommand.h"
+#include "subsystems/ExampleSubsystem.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+ static ExampleSubsystem m_subsystem;
+ static OI m_oi;
+
+ void RobotInit() override;
+ void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
+ void AutonomousInit() override;
+ void AutonomousPeriodic() override;
+ void TeleopInit() override;
+ void TeleopPeriodic() override;
+ void TestPeriodic() override;
+
+ private:
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc::Command* m_autonomousCommand = nullptr;
+ ExampleCommand m_defaultAuto;
+ MyAutoCommand m_myAuto;
+ frc::SendableChooser<frc::Command*> m_chooser;
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/RobotMap.h b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/RobotMap.h
new file mode 100644
index 0000000..875d3aa
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/RobotMap.h
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+/**
+ * The RobotMap is a mapping from the ports sensors and actuators are wired into
+ * to a variable name. This provides flexibility changing wiring, makes checking
+ * the wiring easier and significantly reduces the number of magic numbers
+ * floating around.
+ */
+
+// For example to map the left and right motors, you could define the
+// following variables to use with your drivetrain subsystem.
+// constexpr int kLeftMotor = 1;
+// constexpr int kRightMotor = 2;
+
+// If you are using multiple modules, make sure to define both the port
+// number and the module. For example you with a rangefinder:
+// constexpr int kRangeFinderPort = 1;
+// constexpr int kRangeFinderModule = 1;
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/ExampleCommand.h b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/ExampleCommand.h
new file mode 100644
index 0000000..558fced
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/ExampleCommand.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/commands/Command.h>
+
+class ExampleCommand : public frc::Command {
+ public:
+ ExampleCommand();
+ void Initialize() override;
+ void Execute() override;
+ bool IsFinished() override;
+ void End() override;
+ void Interrupted() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/MyAutoCommand.h b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/MyAutoCommand.h
new file mode 100644
index 0000000..f1892a7
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/MyAutoCommand.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <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/oldcommandbased/include/subsystems/ExampleSubsystem.h b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/subsystems/ExampleSubsystem.h
new file mode 100644
index 0000000..0f7d470
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/subsystems/ExampleSubsystem.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/commands/Subsystem.h>
+
+class ExampleSubsystem : public frc::Subsystem {
+ public:
+ ExampleSubsystem();
+ void InitDefaultCommand() override;
+
+ private:
+ // It's desirable that everything possible under private except
+ // for methods that implement subsystem capabilities
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
index 8495b63..de8fc3e 100644
--- a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
@@ -7,11 +7,10 @@
#include "Robot.h"
-#include <hal/DriverStation.h>
-
#include <frc/DriverStation.h>
#include <frc/livewindow/LiveWindow.h>
#include <frc/shuffleboard/Shuffleboard.h>
+#include <hal/DriverStation.h>
#include <networktables/NetworkTable.h>
void Robot::RobotInit() {}
@@ -32,7 +31,7 @@
// Tell the DS that the robot is ready to be enabled
HAL_ObserveUserProgramStarting();
- while (true) {
+ while (!m_exit) {
if (IsDisabled()) {
m_ds.InDisabled(true);
Disabled();
@@ -61,6 +60,8 @@
}
}
+void Robot::EndCompetition() { m_exit = true; }
+
#ifndef RUNNING_FRC_TESTS
int main() { return frc::StartRobot<Robot>(); }
#endif
diff --git a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h
index 4efc087..0057778 100644
--- a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h
@@ -7,6 +7,8 @@
#pragma once
+#include <atomic>
+
#include <frc/RobotBase.h>
class Robot : public frc::RobotBase {
@@ -18,4 +20,8 @@
void Test();
void StartCompetition() override;
+ void EndCompetition() override;
+
+ private:
+ std::atomic<bool> m_exit{false};
};
diff --git a/wpilibcExamples/src/main/cpp/templates/templates.json b/wpilibcExamples/src/main/cpp/templates/templates.json
index 71798c9..c31d41c 100644
--- a/wpilibcExamples/src/main/cpp/templates/templates.json
+++ b/wpilibcExamples/src/main/cpp/templates/templates.json
@@ -1,21 +1,13 @@
[
{
- "name": "Iterative Robot",
- "description": "Iterative style",
- "tags": [
- "Iterative"
- ],
- "foldername": "iterative",
- "gradlebase": "cpp"
- },
- {
"name": "Timed Robot",
"description": "Timed style",
"tags": [
"Timed"
],
"foldername": "timed",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Timed Skeleton (Advanced)",
@@ -24,7 +16,8 @@
"Timed", "Skeleton"
],
"foldername": "timedskeleton",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "RobotBase Skeleton (Advanced)",
@@ -33,7 +26,8 @@
"RobotBase", "Skeleton"
],
"foldername": "robotbaseskeleton",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 1
},
{
"name": "Command Robot",
@@ -42,6 +36,17 @@
"Command"
],
"foldername": "commandbased",
- "gradlebase": "cpp"
+ "gradlebase": "cpp",
+ "commandversion": 2
+ },
+ {
+ "name": "Old Command Robot",
+ "description": "Old-Command style (deprecated)",
+ "tags": [
+ "Command"
+ ],
+ "foldername": "oldcommandbased",
+ "gradlebase": "cpp",
+ "commandversion": 1
}
]