Squashed 'third_party/allwpilib/' changes from f1a82828fe..ce550705d7
ce550705d7 [ntcore] Fix client "received unknown id -1" (#6186)
3989617bde [ntcore] NetworkTable::GetStruct: Add I template param (#6183)
f1836e1321 [fieldImages] Fix 2024 field json (#6179)
d05f179a9a [build] Fix running apriltagsvision Java example (#6173)
b1b03bed85 [wpilib] Update MotorControllerGroup deprecation message (#6171)
fa63fbf446 LICENSE.md: Bump year to 2024 (#6169)
4809f3d0fc [apriltag] Add 2024 AprilTag locations (#6168)
dd90965362 [wpiutil] Fix RawFrame.setInfo() NPE (#6167)
8659372d08 [fieldImages] Add 2024 field image (#6166)
a2e4d0b15d [docs] Fix docs for SysID routine (#6164)
0a46a3a618 [wpilib] Make ADXL345 default I2C address public (#6163)
7c26bc70ab [sysid] Load DataLog files directly for analysis (#6103)
f94e3d81b9 [docs] Fix SysId routine JavaDoc warnings (#6159)
6bed82a18e [wpilibc] Clean up C++ SysId routine (#6160)
4595f84719 [wpilib] Report LiveWindow-enabled-in-test (#6158)
707cb06105 [wpilib] Add SysIdRoutine logging utility and command factory (#6033)
3e40b9e5da [wpilib] Correct SmartDashboard usage reporting (#6157)
106518c3f8 [docs] Fix wpilibj JavaDoc warnings (#6154)
19cb2a8eb4 [wpilibj] Make class variables private to match C++ (#6153)
13f4460e00 [docs] Add missing docs to enum fields (NFC) (#6150)
4210f5635d [docs] Fix warnings about undocumented default constructors (#6151)
0f060afb55 [ntcore] Disable WebSocket fragmentation (#6149)
f29a7d2e50 [docs] Add missing JavaDocs (#6146)
6e58db398d [commands] Make Java fields private (#6148)
4ac0720385 [build] Clean up CMake files (#6141)
44db3e0ac0 [sysid] Make constexpr variables outside class scope inline (#6145)
73c7d87db7 [glass] NTStringChooser: Properly set retained (#6144)
25636b712f [build] Remove unnecessary native dependencies in wpilibjExamples (#6143)
01fb98baaa [docs] Add Missing JNI docs from C++ (NFC) (#6139)
5c424248c4 [wpilibj] Remove unused AnalogTriggerException (#6142)
c486972c55 [wpimath] Make ExponentialProfile.State mutable (#6138)
783acb9b72 [wpilibj] Store long preferences as integers (#6136)
99ab836894 [wpiutil] Add missing JavaDocs (NFC) (#6132)
ad0859a8c9 [docs] Add missing JavaDocs (#6125)
5579219716 [docs] Exclude quickbuf files and proto/struct packages from doclint (#6128)
98f06911c7 [sysid] Use eigenvector component instead of eigenvalue for fit quality check (#6131)
e1d49b975c [wpimath] Add LinearFilter reset() overload to initialize input and output buffers (#6133)
8a0bf2b7a4 [hal] Add CANAPITypes to java (#6121)
91d8837c11 [wpilib] Make protected fields in accelerometers/gyros private (#6134)
e7c9f27683 [wpilib] Add functional interface equivalents to MotorController (#6053)
8aca706217 [glass] Add type information to SmartDashboard menu (#6117)
7d3e4ddba9 [docs] Add warning about using user button to docs (NFC) (#6129)
ec3cb3dcba [build] Disable clang-tidy warning about test case names (#6127)
495585b25d [examples] Update april tag family to 36h11 (#6126)
f9aabc5ab2 [wpilib] Throw early when EventLoop is modified while running (#6115)
c16946c0ec [hal] Add CANJNI docs (NFC) (#6120)
b7f4eb2811 [doc] Update maven artifacts for units and apriltags (NFC) (#6123)
f419a62b38 [doc] Update maintainers.md (NFC) (#6124)
938bf45fd9 [wpiutil] Remove type param from ProtobufSerializable and StructSerializable (#6122)
c34debe012 [docs] Link to external OpenCV docs (#6119)
07183765de [hal] Fix formatting of HAL_ENUM enums (NFC) (#6114)
af46034b7f [wpilib] Document only first party controllers are guaranteed to have correct mapping (#6112)
636ef58d94 [hal] Properly error check readCANStreamSession (#6108)
cc631d2a69 [build] Fix generated source set location in the HAL (#6113)
09f76b32c2 [wpimath] Compile with UTF-8 encoding (#6111)
47c5fd8620 [sysid] Check data quality before OLS (#6110)
24a76be694 [hal] Add method to detect if the CAN Stream has overflowed (#6105)
9333951736 [hal] Allocate CANStreamMessage in JNI if null (#6107)
6a2d3c30a6 [wpiutil] Struct: Add info template parameter pack (#6086)
e07de37e64 [commands] Mark ParallelDeadlineGroup.setDeadline() final (#6102)
141241d2d6 [wpilib] Fix usage reporting for static classes (#6090)
f2c2bab7dc [sysid] Fix adjusted R² calculation (#6101)
5659038443 [wpiutil,cscore,apriltag] Fix RawFrame (#6098)
8aeee03626 [commands] Improve error message when composing commands twice in same composition (#6091)
55508706ff [wpiutil,cscore] Move VideoMode.PixelFormat to wpiutil (#6097)
ab78b930e9 [wpilib] ADIS16470: Add access to all 3 axes (#6074)
795d4be9fd [wpilib] Fix precision issue in Color round-and-clamp (#6100)
7aa9ad44b8 [commands] Deprecate C++ TransferOwnership() (#6095)
92c81d0791 [ci] Update pregenerate workflow to actions/checkout@v4 (#6094)
1ce617be07 [ci] Update artifact actions to v4 (#6092)
2441b57156 [wpilib] Add PWMSparkFlex MotorController (#6089)
21d1972d7a [wpiutil] DataLog: Ensure file is written on shutdown (#6087)
c29e8c66cf [wpiutil] DataLog: Fix UB in AppendImpl (#6088)
ab309e34ef [glass] Fix order of loading window settings (#6056)
22a322c9f3 [wpimath] Report error on negative PID gains (#6055)
1dba26c937 [wpilib] Add method to get breaker fault at a specific channel in PowerDistribution[Sticky]Faults (#5521)
ef1cb3f41e [commands] Fix compose-while-scheduled issue and test all compositions (#5581)
aeb1a4aa33 [wpiutil] Add serializable marker interfaces (#6060)
c1178d5add [wpilib] Add StadiaController and command wrapper (#6083)
4e4a468d4d [wpimath] Make feedforward classes throw exceptions for negative Kv or Ka (#6084)
d1793f077d [build] cmake: Add NO_WERROR option to disable -Werror (#6071)
43fb6e9f87 [glass] Add Profiled PID controller support & IZone Support (#5959)
bcef6c5398 [apriltag] Fix Java generation functions (#6063)
4059e0cd9f [hal,wpilib] Add function to control "Radio" LED (#6073)
0b2cfb3abc [dlt] Change datalogtool default folder to logs folder (#6079)
df5e439b0c [wpilib] PS4Controller: enable usage reporting (#6081)
0ff7478968 [cscore] Fix RawFrame class not being loaded in JNI (#6077)
6f23d32fe1 [wpilib] AddressableLED: Update warning about single driver (NFC) (#6069)
35a1c52788 [build] Upgrade quickbuf to 1.3.3 (#6072)
e4e2bafdb1 [sysid] Document timestamp units (#6065)
3d201c71f7 [ntcore] Fix overlapping subscriber handling (#6067)
f02984159f [glass] Check for null entries when updating struct/proto (#6059)
a004c9e05f [commands] SubsystemBase: allow setting name in constructor (#6052)
0b4c6a1546 [wpimath] Add more docs to SimulatedAnnealing (NFC) (#6054)
ab15dae887 [wpilib] ArcadeDrive: Fix max output handling (#6051)
9599c1f56f [hal] Add usage reporting ids from 2024v2 image (#6041)
f87c64af8a [wpimath] MecanumDriveWheelSpeeds: Fix desaturate() (#6040)
8798700cec [wpilibcExamples] Add inline specifier to constexpr constants (#6049)
85c9ae6eff [wpilib] Fix PS5 Controller mappings (#6050)
7c8b7a97ad [wpiutil] Zero out roborio system timestamp (#6042)
d9b504bc84 [wpilib] DataLogManager: Change sim location to logs subdir (#6039)
906b810136 [build] cmake: Fix ntcore generated header install (#6038)
56e5b404d1 Update to final 2024 V2 image (#6034)
8723ee5c39 [ntcore] Add cached topic property (#5494)
192a28af47 Fix JDK 21 warnings (#6028)
d40bdd70ba [build] Upgrade to spotbugs Gradle plugin 6.0.2 (#6027)
7bfadf32e5 [wpilibj] Joystick: make remainder of get axis methods final (#6024)
a770110438 [commands] CommandCompositionError: Include stacktrace of original composition (#5984)
54a55b8b53 [wpiutil,hal] Update image; init Rio Now() HMB with a FPGA session (#6016)
7d4e515a6b [wpimath] Simplify calculation of C for DARE precondition (#6022)
5200316c14 [ntcore] Update transmit period on topic add/remove (#6021)
ddf79a25d4 [wpiunits] Overload Measure.per(Time) to return Measure<Velocity> (#6018)
a71adef316 [wpiutil] Clean up circular_buffer iterator syntax (#6020)
39a0bf4b98 [examples] Call resetOdometry() when controller command is executed (#5905)
f5fc101fda [build] cmake: Export jars and clean up jar installs (#6014)
38bf024c96 [build] Update to Gradle 8.5 (#6007)
9d11544c18 [wpimath] Rotate traveling salesman solution so input and solution have same initial pose (#6015)
28deba20f5 [wpimath] Commit generated quickbuf Java files (#5994)
c2971c0bb3 [build] cmake: Export apriltag and wpimath (#6012)
41cfc961e4 gitattributes: Add linguist-generated locations (#6004)
14c3ade155 [wpimath] Struct cleanup (#6011)
90757b9e90 [wpilib] Make Color::HexString() constexpr (#5985)
2676b77873 Fix compilation issues that occur when building with bazel (#6008)
d32c10487c [examples] Update C++ examples to use CommandPtr (#5988)
9bc5fcf886 [build] cmake: Default WITH_JAVA_SOURCE to WITH_JAVA (#6005)
d431abba3b [upstream_utils] Fix GCEM namespace usage and add hypot(x, y, z) (#6002)
2bb1409b82 Clean up Java style (#5990)
66172ab288 Remove submodule (#6003)
e8f8c0ceb0 [upstream_utils] Update to latest Eigen HEAD (#5996)
890992a849 [hal] Commit generated usage reporting files (#5993)
a583ca01e1 [wpiutil] Change Struct to allow non-constexpr implementation (#5992)
ca272de400 [build] Fix Gradle compile_commands.json and clang-tidy warnings (#5977)
76ae090570 [wpiutil] type_traits: Add is_constexpr() (#5997)
5172ab8fd0 [commands] C++ CommandPtr: Prevent null initialization (#5991)
96914143ba [build] Bump native-utils to fix compile_commands.json (#5989)
464e6121ef [ci] Report failed status to Azure on failed tests (#2654)
5dad46cd45 [wpimath] Commit generated files (#5986)
54ab65a63a [ntcore] Commit generated files (#5962)
7ed900ae3a [wpilib] Add hex string constructor to Color and Color8Bit (#5063)
74b85b76a9 [wpimath] Make gcem call std functions if not constant-evaluated (#5983)
30816111db [wpimath] Fix TimeInterpolatableBuffer crash (#5972)
5cc923de33 [wpilib] DataLogManager: Use logs subdirectory on USB drives (#5975)
1144115da0 [commands] Add GetName to Subsystem, use in Scheduler tracer epochs (#5836)
ac7d726ac3 [wpimath] Add simulated annealing (#5961)
e09be72ee0 [wpimath] Remove unused SimpleMatrixUtils class (#5979)
0f9ebe92d9 [wpimath] Add generic circular buffer class to Java (#5969)
9fa28eb07a [ci] Bump actions/checkout to v4 (#5736)
ca684ac207 [hal] Add capability to read power distribution data as a stream (#4983)
51eecef2bd [wpimath] Optimize 2nd derivative of quintic splines (#3292)
4fcf0b25a1 [build] Apply a formatter for CMake files (#5973)
9b8011aa67 [build] Pin wpiformat version (#5982)
e00a0e84c1 [build] cmake: fix protobuf dependency finding for certain distributions (#5981)
23dd591394 [upstream_utils] Remove libuv patch that adjusts whitespace (#5976)
b0719942f0 [wpiutil] Timestamp: Report errors on Rio HMB init failure (#5974)
7bc89c4322 [wpilib] Update getAlliance() docs (NFC) (#5971)
841ea682d1 [upstream_utils] Upgrade to LLVM 17.0.5 (#5970)
a74db52dae [cameraserver] Add getVideo() pixelFormat overload (#5966)
a7eb422662 [build] Update native utils for new compile commands files (#5968)
544b231d4d [sysid] Add missing cassert include (#5967)
31cd015970 [wpimath] Add SysId doc links to LinearSystemId in C++ (NFC) (#5960)
9280054eab Revert "[build] Export wpimath protobuf symbols (#5952)"
2aba97c610 Export pb files from wpimath
c80b2d2017 [build] Export wpimath protobuf symbols (#5952)
3c0652c18a [cscore] Replace CS_PixelFormat with WPI_PixelFormat (#5954)
95716eb0cb [wpiunits] Documentation improvements (#5932)
423fd75fa8 [wpilib] Default LiveWindowEnabledInTest to false (#5950)
dfdea9c992 [wpimath] Make KalmanFilter variant for asymmetric updates (#5951)
ca81ced409 [wpiutil] Move RawFrame to wpiutil; add generation of RawFrame for AprilTags (#5923)
437cc91af5 [cscore] CvSink: Allow specifying output PixelFormat (#5943)
25b7dca46b [build] Remove CMake flat install option (#5944)
bb05e20247 [wpimath] Add protobuf/struct for trivial types (#5935)
35744a036e [wpimath] Move struct/proto classes to separate files (#5918)
80d7ad58ea [build] Declare platform launcher dependency explicitly (#5909)
f8d983b154 [ntcore] Protobuf/Struct: Use atomic_bool instead of atomic_flag (#5946)
4a44210ee3 [ntcore] NetworkTableInstance: Suppress unused lambda capture warning (#5947)
bdc8620d55 [upstream_utils] Fix fmt compilation errors on Windows (#5948)
0ca1e9b5f9 [wpimath] Add basic wpiunits support (#5821)
cc30824409 [ntcore] Increase client meta-topic decoding limit (#5934)
b1fad062f7 [wpilib] Use RKDP in DifferentialDrivetrainSim (#5931)
ead9ae5a69 [build] Add generateProto dependency to test and dev (#5933)
cfbff32185 [wpiutil] timestamp: Fix startup race on Rio (#5930)
7d90d0bcc3 [wpimath] Clean up StateSpaceUtil (#5891)
7755e45aac [build] Add generated protobuf headers to C++ test include path (#5926)
3985c031da [ntcore] ProtobufSubscriber: Fix typos (#5928)
7a87fe4b60 [ntcore] ProtobufSubscriber: Make mutex and msg mutable (#5927)
09f3ed6a5f [commands] Add static Trigger factories for robot mode changes (#5902)
79dd795bc0 [wpimath] Clean up VecBuilder and MatBuilder (#5906)
e117274a67 [wpilib] Change default Rio log dir from /home/lvuser to /home/lvuser/logs (#5899)
a8b80ca256 [upstream_utils] Update to libuv 1.47.0 (#5889)
b3a9c3e96b [build] Bump macOS deployment target to 12 (#5890)
0f8129677b [build] Distribute wpimath protobuf headers (#5925)
d105f9e3e9 [wpiutil] ProtobufBuffer: Fix buffer reallocation (#5924)
c5f2f6a0fb [fieldImages] Fix typo in field images artifact name (#5922)
c1a57e422a [commands] Clean up make_vector.h (#5917)
78ebc6e9ec [wpimath] change G to gearing in LinearSystemId factories (#5834)
9ada181866 [hal] DriverStation.h: Add stddef.h include (#5897)
95fa5ec72f [wpilibc,ntcoreffi] DataLogManager: join on Stop() call (#5910)
b6f2d3cc14 [build] Remove usage of Version.parse (#5911)
cc2cbeb04c [examples] Replace gyro rotation with poseEstimator rotation (#5900)
fa6b171e1c [wpiutil] Suppress protobuf warning false positives on GCC 13 (#5907)
d504639bbe [apriltag] Improve AprilTag docs (#5895)
3a1194be40 Replace static_cast<void>() with [[maybe_unused]] attribute (#5892)
70392cbbcb [build] cmake: Add protobuf dependency to wpiutil-config (#5886)
17c1bd5a83 [ntcore] Use json_fwd (#5881)
e69a9efeba [wpilibcExamples] Match array parameter bounds (#5880)
14dcd0d26f Use char instead of uint8_t for json::parse (#5877)
ec1d261984 [hal] Fix garbage data for match info before DS connection (#5879)
63dbf5c614 [wpiutil] MemoryBuffer: Fix normal read and file type check (#5875)
b2e7be9250 [ntcore] Only datalog meta-topics if specifically requested (#5873)
201a42a3cd [wpimath] Reorder TrapezoidProfile.calculate() arguments (#5874)
04a781b4d7 [apriltag] Add GetTags to C++ version of AprilTagFieldLayout (#5872)
87a8a1ced4 [docs] Exclude eigen and protobuf from doxygen (#5871)
git-subtree-dir: third_party/allwpilib
git-subtree-split: ce550705d7cdab117c0153a202973fc026a81274
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ic8645d0551d62b411b0a816c493f0f33291896a1
diff --git a/wpilibjExamples/build.gradle b/wpilibjExamples/build.gradle
index d5f71e9..4caafc2 100644
--- a/wpilibjExamples/build.gradle
+++ b/wpilibjExamples/build.gradle
@@ -25,9 +25,8 @@
implementation project(':romiVendordep')
implementation project(':xrpVendordep')
- testImplementation 'org.junit.jupiter:junit-jupiter-api:5.10.0'
- testImplementation 'org.junit.jupiter:junit-jupiter-params:5.10.0'
- testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.10.0'
+ testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
+ testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
}
jacoco {
@@ -95,11 +94,8 @@
}
}
binaries.all { binary ->
- lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
- lib project: ':romiVendordep', library: 'romiVendordep', linkage: 'shared'
- lib project: ':xrpVendordep', library: 'xrpVendordep', linkage: 'shared'
lib project: ':apriltag', library: 'apriltag', linkage: 'shared'
- lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+ lib project: ':apriltag', library: 'apriltagJNIShared', linkage: 'shared'
lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
lib project: ':wpimath', library: 'wpimathJNIShared', linkage: 'shared'
project(':ntcore').addNtcoreDependency(binary, 'shared')
@@ -112,7 +108,6 @@
lib project: ':wpiutil', library: 'wpiutilJNIShared', linkage: 'shared'
lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
lib project: ':wpinet', library: 'wpinetJNIShared', linkage: 'shared'
- lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
nativeUtils.useRequiredLibrary(binary, 'ni_link_libraries', 'ni_runtime_libraries')
} else {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java
index 5ed9fda..fa55f45 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java
@@ -22,8 +22,8 @@
// Use current trajectory state here
},
// Goal state
- () -> new TrapezoidProfile.State(),
+ TrapezoidProfile.State::new,
// Current state
- () -> new TrapezoidProfile.State());
+ TrapezoidProfile.State::new);
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java
index a73ea0b..3d265d4 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java
@@ -32,21 +32,21 @@
public class Robot extends TimedRobot {
@Override
public void robotInit() {
- var visionThread = new Thread(() -> apriltagVisionThreadProc());
+ var visionThread = new Thread(this::apriltagVisionThreadProc);
visionThread.setDaemon(true);
visionThread.start();
}
void apriltagVisionThreadProc() {
var detector = new AprilTagDetector();
- // look for tag16h5, don't correct any error bits
- detector.addFamily("tag16h5", 0);
+ // look for tag136h11, correct 7 error bits
+ detector.addFamily("tag36h11", 7);
// Set up Pose Estimator - parameters are for a Microsoft Lifecam HD-3000
// (https://www.chiefdelphi.com/t/wpilib-apriltagdetector-sample-code/421411/21)
var poseEstConfig =
new AprilTagPoseEstimator.Config(
- 0.1524, 699.3778103158814, 677.7161226393544, 345.6059345433618, 207.12741326228522);
+ 0.1651, 699.3778103158814, 677.7161226393544, 345.6059345433618, 207.12741326228522);
var estimator = new AprilTagPoseEstimator(poseEstConfig);
// Get the UsbCamera from CameraServer
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java
index 45333a5..aa34379 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java
@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.examples.arcadedrive;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -16,9 +17,15 @@
public class Robot extends TimedRobot {
private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
- private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final DifferentialDrive m_robotDrive =
+ new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
private final Joystick m_stick = new Joystick(0);
+ public Robot() {
+ SendableRegistry.addChild(m_robotDrive, m_leftMotor);
+ SendableRegistry.addChild(m_robotDrive, m_rightMotor);
+ }
+
@Override
public void robotInit() {
// We need to invert one side of the drivetrain so that positive voltages
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java
index aa0f9a2..a376fe0 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java
@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.examples.arcadedrivexboxcontroller;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -16,9 +17,15 @@
public class Robot extends TimedRobot {
private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
- private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final DifferentialDrive m_robotDrive =
+ new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
private final XboxController m_driverController = new XboxController(0);
+ public Robot() {
+ SendableRegistry.addChild(m_robotDrive, m_leftMotor);
+ SendableRegistry.addChild(m_robotDrive, m_rightMotor);
+ }
+
@Override
public void robotInit() {
// We need to invert one side of the drivetrain so that positive voltages
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
index 4ecaa77..246b5c4 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
@@ -4,28 +4,25 @@
package edu.wpi.first.wpilibj.examples.armbot.subsystems;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
- private final MotorControllerGroup m_leftMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kLeftMotor1Port),
- new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+ private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+ private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
- private final MotorControllerGroup m_rightMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kRightMotor1Port),
- new PWMSparkMax(DriveConstants.kRightMotor2Port));
+ private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+ private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
- private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+ private final DifferentialDrive m_drive =
+ new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
@@ -43,14 +40,20 @@
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ SendableRegistry.addChild(m_drive, m_leftLeader);
+ SendableRegistry.addChild(m_drive, m_rightLeader);
+
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ m_leftLeader.addFollower(m_leftFollower);
+ m_rightLeader.addFollower(m_rightFollower);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
- m_rightMotors.setInverted(true);
+ m_rightLeader.setInverted(true);
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
index 4f95b40..7801a1f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
@@ -4,14 +4,12 @@
package edu.wpi.first.wpilibj.examples.armbotoffboard;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-
/**
* A simplified stub class that simulates the API of a common "smart" motor controller.
*
* <p>Has no actual functionality.
*/
-public class ExampleSmartMotorController implements MotorController {
+public class ExampleSmartMotorController {
public enum PIDMode {
kPosition,
kVelocity,
@@ -72,25 +70,19 @@
/** Resets the encoder to zero distance. */
public void resetEncoder() {}
- @Override
public void set(double speed) {}
- @Override
public double get() {
return 0;
}
- @Override
public void setInverted(boolean isInverted) {}
- @Override
public boolean getInverted() {
return false;
}
- @Override
public void disable() {}
- @Override
public void stopMotor() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
index e3c9159..f01ad99 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
@@ -4,10 +4,10 @@
package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
@@ -16,19 +16,16 @@
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
- private final MotorControllerGroup m_leftMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kLeftMotor1Port),
- new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+ private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+ private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
- private final MotorControllerGroup m_rightMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kRightMotor1Port),
- new PWMSparkMax(DriveConstants.kRightMotor2Port));
+ private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+ private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
- private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+ private final DifferentialDrive m_drive =
+ new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
@@ -46,14 +43,20 @@
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ SendableRegistry.addChild(m_drive, m_leftLeader);
+ SendableRegistry.addChild(m_drive, m_rightLeader);
+
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ m_leftLeader.addFollower(m_leftFollower);
+ m_rightLeader.addFollower(m_rightFollower);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
- m_rightMotors.setInverted(true);
+ m_rightLeader.setInverted(true);
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
index 8b4734a..adc52e2 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
@@ -12,8 +12,6 @@
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/** Represents a differential drive style drivetrain. */
@@ -25,19 +23,14 @@
private static final double kWheelRadius = 0.0508; // meters
private static final int kEncoderResolution = 4096;
- private final MotorController m_leftLeader = new PWMSparkMax(1);
- private final MotorController m_leftFollower = new PWMSparkMax(2);
- private final MotorController m_rightLeader = new PWMSparkMax(3);
- private final MotorController m_rightFollower = new PWMSparkMax(4);
+ private final PWMSparkMax m_leftLeader = new PWMSparkMax(1);
+ private final PWMSparkMax m_leftFollower = new PWMSparkMax(2);
+ private final PWMSparkMax m_rightLeader = new PWMSparkMax(3);
+ private final PWMSparkMax m_rightFollower = new PWMSparkMax(4);
private final Encoder m_leftEncoder = new Encoder(0, 1);
private final Encoder m_rightEncoder = new Encoder(2, 3);
- private final MotorControllerGroup m_leftGroup =
- new MotorControllerGroup(m_leftLeader, m_leftFollower);
- private final MotorControllerGroup m_rightGroup =
- new MotorControllerGroup(m_rightLeader, m_rightFollower);
-
private final AnalogGyro m_gyro = new AnalogGyro(0);
private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
@@ -58,10 +51,13 @@
public Drivetrain() {
m_gyro.reset();
+ m_leftLeader.addFollower(m_leftFollower);
+ m_rightLeader.addFollower(m_rightFollower);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
- m_rightGroup.setInverted(true);
+ m_rightLeader.setInverted(true);
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
@@ -90,8 +86,8 @@
m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
final double rightOutput =
m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
- m_leftGroup.setVoltage(leftOutput + leftFeedforward);
- m_rightGroup.setVoltage(rightOutput + rightFeedforward);
+ m_leftLeader.setVoltage(leftOutput + leftFeedforward);
+ m_rightLeader.setVoltage(rightOutput + rightFeedforward);
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
index ceceaf3..9b7ec75 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
@@ -30,8 +30,6 @@
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
@@ -48,19 +46,14 @@
private static final double kWheelRadius = 0.0508; // meters
private static final int kEncoderResolution = 4096;
- private final MotorController m_leftLeader = new PWMSparkMax(1);
- private final MotorController m_leftFollower = new PWMSparkMax(2);
- private final MotorController m_rightLeader = new PWMSparkMax(3);
- private final MotorController m_rightFollower = new PWMSparkMax(4);
+ private final PWMSparkMax m_leftLeader = new PWMSparkMax(1);
+ private final PWMSparkMax m_leftFollower = new PWMSparkMax(2);
+ private final PWMSparkMax m_rightLeader = new PWMSparkMax(3);
+ private final PWMSparkMax m_rightFollower = new PWMSparkMax(4);
private final Encoder m_leftEncoder = new Encoder(0, 1);
private final Encoder m_rightEncoder = new Encoder(2, 3);
- private final MotorControllerGroup m_leftGroup =
- new MotorControllerGroup(m_leftLeader, m_leftFollower);
- private final MotorControllerGroup m_rightGroup =
- new MotorControllerGroup(m_rightLeader, m_rightFollower);
-
private final AnalogGyro m_gyro = new AnalogGyro(0);
private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
@@ -113,10 +106,13 @@
public Drivetrain(DoubleArrayTopic cameraToObjectTopic) {
m_gyro.reset();
+ m_leftLeader.addFollower(m_leftFollower);
+ m_rightLeader.addFollower(m_rightFollower);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
- m_rightGroup.setInverted(true);
+ m_rightLeader.setInverted(true);
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
@@ -148,8 +144,8 @@
m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
final double rightOutput =
m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
- m_leftGroup.setVoltage(leftOutput + leftFeedforward);
- m_rightGroup.setVoltage(rightOutput + rightFeedforward);
+ m_leftLeader.setVoltage(leftOutput + leftFeedforward);
+ m_rightLeader.setVoltage(rightOutput + rightFeedforward);
}
/**
@@ -251,8 +247,8 @@
// simulation, and write the simulated positions and velocities to our
// simulated encoder and gyro.
m_drivetrainSimulator.setInputs(
- m_leftGroup.get() * RobotController.getInputVoltage(),
- m_rightGroup.get() * RobotController.getInputVoltage());
+ m_leftLeader.get() * RobotController.getInputVoltage(),
+ m_rightLeader.get() * RobotController.getInputVoltage());
m_drivetrainSimulator.update(0.02);
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java
index 487974c..12c51ab 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java
@@ -4,14 +4,12 @@
package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-
/**
* A simplified stub class that simulates the API of a common "smart" motor controller.
*
* <p>Has no actual functionality.
*/
-public class ExampleSmartMotorController implements MotorController {
+public class ExampleSmartMotorController {
public enum PIDMode {
kPosition,
kVelocity,
@@ -74,27 +72,21 @@
/** Resets the encoder to zero distance. */
public void resetEncoder() {}
- @Override
public void set(double speed) {
m_value = speed;
}
- @Override
public double get() {
return m_value;
}
- @Override
public void setInverted(boolean isInverted) {}
- @Override
public boolean getInverted() {
return false;
}
- @Override
public void disable() {}
- @Override
public void stopMotor() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
index ab71ff4..ba2ed04 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
@@ -78,7 +78,7 @@
// End at desired position in meters; implicitly starts at 0
() -> new TrapezoidProfile.State(3, 0),
// Current position
- () -> new TrapezoidProfile.State(),
+ TrapezoidProfile.State::new,
// Require the drive
m_robotDrive)
.beforeStarting(m_robotDrive::resetEncoders)
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
index 5c107d2..951cde3 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
@@ -29,7 +29,7 @@
// End at desired position in meters; implicitly starts at 0
() -> new TrapezoidProfile.State(meters, 0),
// Current position
- () -> new TrapezoidProfile.State(),
+ TrapezoidProfile.State::new,
// Require the drive
drive);
// Reset drive encoders since we're starting at 0
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
index 520261d..59d700b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
@@ -6,6 +6,7 @@
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.ExampleSmartMotorController;
@@ -34,20 +35,19 @@
DriveConstants.kaVoltSecondsSquaredPerMeter);
// The robot's drive
- private final DifferentialDrive m_drive = new DifferentialDrive(m_leftLeader, m_rightLeader);
+ private final DifferentialDrive m_drive =
+ new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ SendableRegistry.addChild(m_drive, m_leftLeader);
+ SendableRegistry.addChild(m_drive, m_rightLeader);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightLeader.setInverted(true);
- // You might need to not do this depending on the specific motor controller
- // that you are using -- contact the respective vendor's documentation for
- // more details.
- m_rightFollower.setInverted(true);
-
m_leftFollower.follow(m_leftLeader);
m_rightFollower.follow(m_rightLeader);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/ExampleSmartMotorController.java
index e252f85..2b37b96 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/ExampleSmartMotorController.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/ExampleSmartMotorController.java
@@ -4,14 +4,12 @@
package edu.wpi.first.wpilibj.examples.elevatorexponentialprofile;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-
/**
* A simplified stub class that simulates the API of a common "smart" motor controller.
*
* <p>Has no actual functionality.
*/
-public class ExampleSmartMotorController implements MotorController {
+public class ExampleSmartMotorController {
public enum PIDMode {
kPosition,
kVelocity,
@@ -72,25 +70,19 @@
/** Resets the encoder to zero distance. */
public void resetEncoder() {}
- @Override
public void set(double speed) {}
- @Override
public double get() {
return 0;
}
- @Override
public void setInverted(boolean isInverted) {}
- @Override
public boolean getInverted() {
return false;
}
- @Override
public void disable() {}
- @Override
public void stopMotor() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
index b6228c9..670fd7b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
@@ -10,7 +10,6 @@
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
@SuppressWarnings("PMD.RedundantFieldInitializer")
@@ -27,7 +26,7 @@
private final Joystick m_joystick = new Joystick(1);
private final Encoder m_encoder = new Encoder(1, 2);
- private final MotorController m_motor = new PWMSparkMax(1);
+ private final PWMSparkMax m_motor = new PWMSparkMax(1);
// Create a PID controller whose setpoint's change is subject to maximum
// velocity and acceleration constraints.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java
index a1366e6..6d9ac23 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java
@@ -4,14 +4,12 @@
package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-
/**
* A simplified stub class that simulates the API of a common "smart" motor controller.
*
* <p>Has no actual functionality.
*/
-public class ExampleSmartMotorController implements MotorController {
+public class ExampleSmartMotorController {
public enum PIDMode {
kPosition,
kVelocity,
@@ -72,25 +70,19 @@
/** Resets the encoder to zero distance. */
public void resetEncoder() {}
- @Override
public void set(double speed) {}
- @Override
public double get() {
return 0;
}
- @Override
public void setInverted(boolean isInverted) {}
- @Override
public boolean getInverted() {
return false;
}
- @Override
public void disable() {}
- @Override
public void stopMotor() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
index 2acb5bc..938132b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
@@ -40,7 +40,7 @@
// Retrieve the profiled setpoint for the next timestep. This setpoint moves
// toward the goal while obeying the constraints.
- m_setpoint = m_profile.calculate(kDt, m_goal, m_setpoint);
+ m_setpoint = m_profile.calculate(kDt, m_setpoint, m_goal);
// Send setpoint to offboard controller PID
m_motor.setSetpoint(
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Main.java
index d6f3150..23a49cb 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Main.java
@@ -5,7 +5,6 @@
package edu.wpi.first.wpilibj.examples.eventloop;
import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj.examples.encoder.Robot;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java
index 03c5577..08de65c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java
@@ -12,7 +12,6 @@
import edu.wpi.first.wpilibj.Ultrasonic;
import edu.wpi.first.wpilibj.event.BooleanEvent;
import edu.wpi.first.wpilibj.event.EventLoop;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
public class Robot extends TimedRobot {
@@ -20,15 +19,15 @@
public static final int TOLERANCE = 8; // rpm
public static final int KICKER_THRESHOLD = 15; // mm
- private final MotorController m_shooter = new PWMSparkMax(0);
+ private final PWMSparkMax m_shooter = new PWMSparkMax(0);
private final Encoder m_shooterEncoder = new Encoder(0, 1);
private final PIDController m_controller = new PIDController(0.3, 0, 0);
private final SimpleMotorFeedforward m_ff = new SimpleMotorFeedforward(0.1, 0.065);
- private final MotorController m_kicker = new PWMSparkMax(1);
+ private final PWMSparkMax m_kicker = new PWMSparkMax(1);
private final Ultrasonic m_kickerSensor = new Ultrasonic(2, 3);
- private final MotorController m_intake = new PWMSparkMax(2);
+ private final PWMSparkMax m_intake = new PWMSparkMax(2);
private final EventLoop m_loop = new EventLoop();
private final Joystick m_joystick = new Joystick(0);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
index 750e4dc..b1ddb6c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
@@ -983,5 +983,18 @@
"gradlebase": "java",
"commandversion": 2,
"mainclass": "Main"
+ },
+ {
+ "name": "SysIdRoutine",
+ "description": "A sample command-based robot demonstrating use of the SysIdRoutine command factory",
+ "tags": [
+ "SysId",
+ "Command-based",
+ "DataLog"
+ ],
+ "foldername": "sysid",
+ "gradlebase": "java",
+ "commandversion": 2,
+ "mainclass": "Main"
}
]
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
index 5773da1..dc3a3c6 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
@@ -4,28 +4,25 @@
package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
- private final MotorControllerGroup m_leftMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kLeftMotor1Port),
- new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+ private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+ private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
- private final MotorControllerGroup m_rightMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kRightMotor1Port),
- new PWMSparkMax(DriveConstants.kRightMotor2Port));
+ private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+ private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
- private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+ private final DifferentialDrive m_drive =
+ new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
@@ -43,10 +40,16 @@
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ SendableRegistry.addChild(m_drive, m_leftLeader);
+ SendableRegistry.addChild(m_drive, m_rightLeader);
+
+ m_leftLeader.addFollower(m_leftFollower);
+ m_rightLeader.addFollower(m_rightFollower);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
- m_rightMotors.setInverted(true);
+ m_rightLeader.setInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java
index ffde63d..b632f8d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java
@@ -4,34 +4,27 @@
package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.gearsbot.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Drivetrain extends SubsystemBase {
- /**
- * The Drivetrain subsystem incorporates the sensors and actuators attached to the robots chassis.
- * These include four drive motors, a left and right encoder and a gyro.
- */
- private final MotorController m_leftMotor =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kLeftMotorPort1),
- new PWMSparkMax(DriveConstants.kLeftMotorPort1));
+ // The Drivetrain subsystem incorporates the sensors and actuators attached to the robots chassis.
+ // These include four drive motors, a left and right encoder and a gyro.
+ private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotorPort1);
+ private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotorPort2);
+ private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotorPort1);
+ private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotorPort2);
- private final MotorController m_rightMotor =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kRightMotorPort2),
- new PWMSparkMax(DriveConstants.kLeftMotorPort2));
-
- private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final DifferentialDrive m_drive =
+ new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
private final Encoder m_leftEncoder =
new Encoder(
@@ -50,10 +43,16 @@
public Drivetrain() {
super();
+ SendableRegistry.addChild(m_drive, m_leftLeader);
+ SendableRegistry.addChild(m_drive, m_rightLeader);
+
+ m_leftLeader.addFollower(m_leftFollower);
+ m_rightLeader.addFollower(m_rightFollower);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
- m_rightMotor.setInverted(true);
+ m_rightLeader.setInverted(true);
// Encoders may measure differently in the real world and in
// simulation. In this example the robot moves 0.042 barleycorns
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java
index dd337e6..563235c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java
@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.examples.gettingstarted;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.XboxController;
@@ -19,10 +20,16 @@
public class Robot extends TimedRobot {
private final PWMSparkMax m_leftDrive = new PWMSparkMax(0);
private final PWMSparkMax m_rightDrive = new PWMSparkMax(1);
- private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftDrive, m_rightDrive);
+ private final DifferentialDrive m_robotDrive =
+ new DifferentialDrive(m_leftDrive::set, m_rightDrive::set);
private final XboxController m_controller = new XboxController(0);
private final Timer m_timer = new Timer();
+ public Robot() {
+ SendableRegistry.addChild(m_robotDrive, m_leftDrive);
+ SendableRegistry.addChild(m_robotDrive, m_rightDrive);
+ }
+
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java
index 1f9e41d..7d00c27 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java
@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.examples.gyro;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
@@ -30,10 +31,16 @@
private final PWMSparkMax m_leftDrive = new PWMSparkMax(kLeftMotorPort);
private final PWMSparkMax m_rightDrive = new PWMSparkMax(kRightMotorPort);
- private final DifferentialDrive m_myRobot = new DifferentialDrive(m_leftDrive, m_rightDrive);
+ private final DifferentialDrive m_robotDrive =
+ new DifferentialDrive(m_leftDrive::set, m_rightDrive::set);
private final AnalogGyro m_gyro = new AnalogGyro(kGyroPort);
private final Joystick m_joystick = new Joystick(kJoystickPort);
+ public Robot() {
+ SendableRegistry.addChild(m_robotDrive, m_leftDrive);
+ SendableRegistry.addChild(m_robotDrive, m_rightDrive);
+ }
+
@Override
public void robotInit() {
m_gyro.setSensitivity(kVoltsPerDegreePerSecond);
@@ -50,6 +57,6 @@
@Override
public void teleopPeriodic() {
double turningValue = (kAngleSetpoint - m_gyro.getAngle()) * kP;
- m_myRobot.arcadeDrive(-m_joystick.getY(), -turningValue);
+ m_robotDrive.arcadeDrive(-m_joystick.getY(), -turningValue);
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
index 9adb6ec..db87410 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
@@ -4,29 +4,26 @@
package edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
- private final MotorControllerGroup m_leftMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kLeftMotor1Port),
- new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+ private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+ private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
- private final MotorControllerGroup m_rightMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kRightMotor1Port),
- new PWMSparkMax(DriveConstants.kRightMotor2Port));
+ private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+ private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
- private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+ private final DifferentialDrive m_drive =
+ new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
@@ -47,10 +44,16 @@
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ SendableRegistry.addChild(m_drive, m_leftLeader);
+ SendableRegistry.addChild(m_drive, m_rightLeader);
+
+ m_leftLeader.addFollower(m_leftFollower);
+ m_rightLeader.addFollower(m_rightFollower);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
- m_rightMotors.setInverted(true);
+ m_rightLeader.setInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java
index b86277b..a435232 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java
@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.examples.gyromecanum;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
@@ -37,12 +38,17 @@
PWMSparkMax frontRight = new PWMSparkMax(kFrontRightChannel);
PWMSparkMax rearRight = new PWMSparkMax(kRearRightChannel);
+ SendableRegistry.addChild(m_robotDrive, frontLeft);
+ SendableRegistry.addChild(m_robotDrive, rearLeft);
+ SendableRegistry.addChild(m_robotDrive, frontRight);
+ SendableRegistry.addChild(m_robotDrive, rearRight);
+
// Invert the right side motors.
// You may need to change or remove this to match your robot.
frontRight.setInverted(true);
rearRight.setInverted(true);
- m_robotDrive = new MecanumDrive(frontLeft, rearLeft, frontRight, rearRight);
+ m_robotDrive = new MecanumDrive(frontLeft::set, rearLeft::set, frontRight::set, rearRight::set);
m_gyro.setSensitivity(kVoltsPerDegreePerSecond);
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
index 1376195..8de4a90 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
@@ -5,28 +5,25 @@
package edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems;
import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
- private final MotorControllerGroup m_leftMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kLeftMotor1Port),
- new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+ private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+ private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
- private final MotorControllerGroup m_rightMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kRightMotor1Port),
- new PWMSparkMax(DriveConstants.kRightMotor2Port));
+ private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+ private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
- private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+ private final DifferentialDrive m_drive =
+ new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
@@ -44,10 +41,16 @@
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ SendableRegistry.addChild(m_drive, m_leftLeader);
+ SendableRegistry.addChild(m_drive, m_rightLeader);
+
+ m_leftLeader.addFollower(m_leftFollower);
+ m_rightLeader.addFollower(m_rightFollower);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
- m_rightMotors.setInverted(true);
+ m_rightLeader.setInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
index 8a5296d..8942b41 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
@@ -5,28 +5,25 @@
package edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems;
import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
- private final MotorControllerGroup m_leftMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kLeftMotor1Port),
- new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+ private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+ private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
- private final MotorControllerGroup m_rightMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kRightMotor1Port),
- new PWMSparkMax(DriveConstants.kRightMotor2Port));
+ private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+ private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
- private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+ private final DifferentialDrive m_drive =
+ new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
@@ -44,10 +41,16 @@
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ SendableRegistry.addChild(m_drive, m_leftLeader);
+ SendableRegistry.addChild(m_drive, m_rightLeader);
+
+ m_leftLeader.addFollower(m_leftFollower);
+ m_rightLeader.addFollower(m_rightFollower);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
- m_rightMotors.setInverted(true);
+ m_rightLeader.setInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
index 15e813a..c64c9b3 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
@@ -14,7 +14,6 @@
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/** Represents a mecanum drive style drivetrain. */
@@ -22,10 +21,10 @@
public static final double kMaxSpeed = 3.0; // 3 meters per second
public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
- private final MotorController m_frontLeftMotor = new PWMSparkMax(1);
- private final MotorController m_frontRightMotor = new PWMSparkMax(2);
- private final MotorController m_backLeftMotor = new PWMSparkMax(3);
- private final MotorController m_backRightMotor = new PWMSparkMax(4);
+ private final PWMSparkMax m_frontLeftMotor = new PWMSparkMax(1);
+ private final PWMSparkMax m_frontRightMotor = new PWMSparkMax(2);
+ private final PWMSparkMax m_backLeftMotor = new PWMSparkMax(3);
+ private final PWMSparkMax m_backRightMotor = new PWMSparkMax(4);
private final Encoder m_frontLeftEncoder = new Encoder(0, 1);
private final Encoder m_frontRightEncoder = new Encoder(2, 3);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
index 74c088f..d278f05 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
@@ -19,6 +19,7 @@
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.MecanumControllerCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -85,7 +86,7 @@
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(DriveConstants.kDriveKinematics);
- // An example trajectory to follow. All units in meters.
+ // An example trajectory to follow. All units in meters.
Trajectory exampleTrajectory =
TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
@@ -121,10 +122,11 @@
m_robotDrive::setDriveMotorControllersVolts, // Consumer for the output motor voltages
m_robotDrive);
- // Reset odometry to the starting pose of the trajectory.
- m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
-
- // Run path following command, then stop at the end.
- return mecanumControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
+ // Reset odometry to the initial pose of the trajectory, run path following
+ // command, then stop at the end.
+ return Commands.sequence(
+ new InstantCommand(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose())),
+ mecanumControllerCommand,
+ new InstantCommand(() -> m_robotDrive.drive(0, 0, 0, false)));
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
index 6d731ba..b3f9b34 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
@@ -9,6 +9,7 @@
import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
@@ -23,7 +24,7 @@
private final PWMSparkMax m_rearRight = new PWMSparkMax(DriveConstants.kRearRightMotorPort);
private final MecanumDrive m_drive =
- new MecanumDrive(m_frontLeft, m_rearLeft, m_frontRight, m_rearRight);
+ new MecanumDrive(m_frontLeft::set, m_rearLeft::set, m_frontRight::set, m_rearRight::set);
// The front-left-side drive encoder
private final Encoder m_frontLeftEncoder =
@@ -65,6 +66,11 @@
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ SendableRegistry.addChild(m_drive, m_frontLeft);
+ SendableRegistry.addChild(m_drive, m_rearLeft);
+ SendableRegistry.addChild(m_drive, m_frontRight);
+ SendableRegistry.addChild(m_drive, m_rearRight);
+
// Sets the distance per pulse for the encoders
m_frontLeftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rearLeftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java
index f376543..77aa0ce 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java
@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.examples.mecanumdrive;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
@@ -28,12 +29,17 @@
PWMSparkMax frontRight = new PWMSparkMax(kFrontRightChannel);
PWMSparkMax rearRight = new PWMSparkMax(kRearRightChannel);
+ SendableRegistry.addChild(m_robotDrive, frontLeft);
+ SendableRegistry.addChild(m_robotDrive, rearLeft);
+ SendableRegistry.addChild(m_robotDrive, frontRight);
+ SendableRegistry.addChild(m_robotDrive, rearRight);
+
// Invert the right side motors.
// You may need to change or remove this to match your robot.
frontRight.setInverted(true);
rearRight.setInverted(true);
- m_robotDrive = new MecanumDrive(frontLeft, rearLeft, frontRight, rearRight);
+ m_robotDrive = new MecanumDrive(frontLeft::set, rearLeft::set, frontRight::set, rearRight::set);
m_stick = new Joystick(kJoystickChannel);
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
index 4b643bd..0625b3f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
@@ -18,7 +18,6 @@
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/** Represents a mecanum drive style drivetrain. */
@@ -26,10 +25,10 @@
public static final double kMaxSpeed = 3.0; // 3 meters per second
public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
- private final MotorController m_frontLeftMotor = new PWMSparkMax(1);
- private final MotorController m_frontRightMotor = new PWMSparkMax(2);
- private final MotorController m_backLeftMotor = new PWMSparkMax(3);
- private final MotorController m_backRightMotor = new PWMSparkMax(4);
+ private final PWMSparkMax m_frontLeftMotor = new PWMSparkMax(1);
+ private final PWMSparkMax m_frontRightMotor = new PWMSparkMax(2);
+ private final PWMSparkMax m_backLeftMotor = new PWMSparkMax(3);
+ private final PWMSparkMax m_backRightMotor = new PWMSparkMax(4);
private final Encoder m_frontLeftEncoder = new Encoder(0, 1);
private final Encoder m_frontRightEncoder = new Encoder(2, 3);
@@ -147,7 +146,7 @@
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
- xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+ xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
periodSeconds));
mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java
index 82ced20..cbb4bab 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java
@@ -7,7 +7,6 @@
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -27,7 +26,7 @@
private static final int kEncoderPortA = 0;
private static final int kEncoderPortB = 1;
- private MotorController m_motor;
+ private PWMSparkMax m_motor;
private Joystick m_joystick;
private Encoder m_encoder;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
index 9f0980a..4bbf45c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
@@ -4,8 +4,6 @@
package edu.wpi.first.wpilibj.examples.ramsetecommand;
-import static edu.wpi.first.wpilibj.XboxController.Button;
-
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.RamseteController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
@@ -17,11 +15,13 @@
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.XboxController.Button;
import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants;
import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -97,7 +97,7 @@
// Apply the voltage constraint
.addConstraint(autoVoltageConstraint);
- // An example trajectory to follow. All units in meters.
+ // An example trajectory to follow. All units in meters.
Trajectory exampleTrajectory =
TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
@@ -126,10 +126,10 @@
m_robotDrive::tankDriveVolts,
m_robotDrive);
- // Reset odometry to the starting pose of the trajectory.
- m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
-
- // Run path following command, then stop at the end.
- return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));
+ // Reset odometry to the initial pose of the trajectory, run path following
+ // command, then stop at the end.
+ return Commands.runOnce(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose()))
+ .andThen(ramseteCommand)
+ .andThen(Commands.runOnce(() -> m_robotDrive.tankDriveVolts(0, 0)));
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
index 288baed..f2b0840 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
@@ -7,29 +7,26 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
- private final MotorControllerGroup m_leftMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kLeftMotor1Port),
- new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+ private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+ private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
- private final MotorControllerGroup m_rightMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kRightMotor1Port),
- new PWMSparkMax(DriveConstants.kRightMotor2Port));
+ private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+ private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
- private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+ private final DifferentialDrive m_drive =
+ new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
@@ -53,10 +50,16 @@
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ SendableRegistry.addChild(m_drive, m_leftLeader);
+ SendableRegistry.addChild(m_drive, m_rightLeader);
+
+ m_leftLeader.addFollower(m_leftFollower);
+ m_rightLeader.addFollower(m_rightFollower);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
- m_rightMotors.setInverted(true);
+ m_rightLeader.setInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -121,8 +124,8 @@
* @param rightVolts the commanded right output
*/
public void tankDriveVolts(double leftVolts, double rightVolts) {
- m_leftMotors.setVoltage(leftVolts);
- m_rightMotors.setVoltage(rightVolts);
+ m_leftLeader.setVoltage(leftVolts);
+ m_rightLeader.setVoltage(rightVolts);
m_drive.feed();
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java
index 69288a1..98d1856 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java
@@ -13,8 +13,6 @@
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/** Represents a differential drive style drivetrain. */
@@ -26,19 +24,14 @@
private static final double kWheelRadius = 0.0508; // meters
private static final int kEncoderResolution = 4096;
- private final MotorController m_leftLeader = new PWMSparkMax(1);
- private final MotorController m_leftFollower = new PWMSparkMax(2);
- private final MotorController m_rightLeader = new PWMSparkMax(3);
- private final MotorController m_rightFollower = new PWMSparkMax(4);
+ private final PWMSparkMax m_leftLeader = new PWMSparkMax(1);
+ private final PWMSparkMax m_leftFollower = new PWMSparkMax(2);
+ private final PWMSparkMax m_rightLeader = new PWMSparkMax(3);
+ private final PWMSparkMax m_rightFollower = new PWMSparkMax(4);
private final Encoder m_leftEncoder = new Encoder(0, 1);
private final Encoder m_rightEncoder = new Encoder(2, 3);
- private final MotorControllerGroup m_leftGroup =
- new MotorControllerGroup(m_leftLeader, m_leftFollower);
- private final MotorControllerGroup m_rightGroup =
- new MotorControllerGroup(m_rightLeader, m_rightFollower);
-
private final AnalogGyro m_gyro = new AnalogGyro(0);
private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
@@ -59,10 +52,13 @@
public Drivetrain() {
m_gyro.reset();
+ m_leftLeader.addFollower(m_leftFollower);
+ m_rightLeader.addFollower(m_rightFollower);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
- m_rightGroup.setInverted(true);
+ m_rightLeader.setInverted(true);
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
@@ -91,8 +87,8 @@
m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
final double rightOutput =
m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
- m_leftGroup.setVoltage(leftOutput + leftFeedforward);
- m_rightGroup.setVoltage(rightOutput + rightFeedforward);
+ m_leftLeader.setVoltage(leftOutput + leftFeedforward);
+ m_rightLeader.setVoltage(rightOutput + rightFeedforward);
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java
index 736ff44..dd7854d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java
@@ -4,10 +4,10 @@
package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -15,19 +15,16 @@
public class Drive extends SubsystemBase {
// The motors on the left side of the drive.
- private final MotorControllerGroup m_leftMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kLeftMotor1Port),
- new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+ private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+ private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
- private final MotorControllerGroup m_rightMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kRightMotor1Port),
- new PWMSparkMax(DriveConstants.kRightMotor2Port));
+ private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+ private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
- private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+ private final DifferentialDrive m_drive =
+ new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
@@ -45,10 +42,16 @@
/** Creates a new Drive subsystem. */
public Drive() {
+ SendableRegistry.addChild(m_drive, m_leftLeader);
+ SendableRegistry.addChild(m_drive, m_rightLeader);
+
+ m_leftLeader.addFollower(m_leftFollower);
+ m_rightLeader.addFollower(m_rightFollower);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
- m_rightMotors.setInverted(true);
+ m_rightLeader.setInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Pneumatics.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Pneumatics.java
index 69ec023..b51562f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Pneumatics.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Pneumatics.java
@@ -50,16 +50,12 @@
*/
public Command disableCompressorCommand() {
return startEnd(
- () -> {
- // Disable closed-loop mode on the compressor.
- m_compressor.disable();
- },
- () -> {
- // Enable closed-loop mode based on the digital pressure switch connected to the
- // PCM/PH.
- // The switch is open when the pressure is over ~120 PSI.
- m_compressor.enableDigital();
- })
+ // Disable closed-loop mode on the compressor.
+ m_compressor::disable,
+ // Enable closed-loop mode based on the digital pressure switch connected to the
+ // PCM/PH.
+ // The switch is open when the pressure is over ~120 PSI.
+ m_compressor::enableDigital)
.withName("Compressor Disabled");
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java
index 0e7f9b5..d282c20 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java
@@ -10,7 +10,7 @@
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants;
+import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.ShooterConstants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java
index d805590..42cf45e 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java
@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.examples.romireference.subsystems;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.BuiltInAccelerometer;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -26,7 +27,8 @@
private final Encoder m_rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
- private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final DifferentialDrive m_diffDrive =
+ new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
// Set up the RomiGyro
private final RomiGyro m_gyro = new RomiGyro();
@@ -36,6 +38,9 @@
/** Creates a new Drivetrain. */
public Drivetrain() {
+ SendableRegistry.addChild(m_diffDrive, m_leftMotor);
+ SendableRegistry.addChild(m_diffDrive, m_rightMotor);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java
index 47109e0..436d726 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java
@@ -5,6 +5,7 @@
package edu.wpi.first.wpilibj.examples.shuffleboard;
import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.TimedRobot;
@@ -16,8 +17,10 @@
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
public class Robot extends TimedRobot {
+ private final PWMSparkMax m_leftDriveMotor = new PWMSparkMax(0);
+ private final PWMSparkMax m_rightDriveMotor = new PWMSparkMax(1);
private final DifferentialDrive m_tankDrive =
- new DifferentialDrive(new PWMSparkMax(0), new PWMSparkMax(1));
+ new DifferentialDrive(m_leftDriveMotor::set, m_rightDriveMotor::set);
private final Encoder m_leftEncoder = new Encoder(0, 1);
private final Encoder m_rightEncoder = new Encoder(2, 3);
@@ -27,6 +30,9 @@
@Override
public void robotInit() {
+ SendableRegistry.addChild(m_tankDrive, m_leftDriveMotor);
+ SendableRegistry.addChild(m_tankDrive, m_rightDriveMotor);
+
// Add a 'max speed' widget to a tab named 'Configuration', using a number slider
// The widget will be placed in the second column and row and will be TWO columns wide
m_maxSpeed =
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
index a9f53a9..fcae928 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
@@ -18,7 +18,6 @@
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
@@ -41,11 +40,6 @@
private final PWMSparkMax m_rightLeader = new PWMSparkMax(3);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(4);
- private final MotorControllerGroup m_leftGroup =
- new MotorControllerGroup(m_leftLeader, m_leftFollower);
- private final MotorControllerGroup m_rightGroup =
- new MotorControllerGroup(m_rightLeader, m_rightFollower);
-
private final Encoder m_leftEncoder = new Encoder(0, 1);
private final Encoder m_rightEncoder = new Encoder(2, 3);
@@ -77,10 +71,13 @@
/** Subsystem constructor. */
public Drivetrain() {
+ m_leftLeader.addFollower(m_leftFollower);
+ m_rightLeader.addFollower(m_rightFollower);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
- m_rightGroup.setInverted(true);
+ m_rightLeader.setInverted(true);
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
@@ -91,7 +88,7 @@
m_leftEncoder.reset();
m_rightEncoder.reset();
- m_rightGroup.setInverted(true);
+ m_rightLeader.setInverted(true);
SmartDashboard.putData("Field", m_fieldSim);
}
@@ -104,8 +101,8 @@
double rightOutput =
m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
- m_leftGroup.setVoltage(leftOutput + leftFeedforward);
- m_rightGroup.setVoltage(rightOutput + rightFeedforward);
+ m_leftLeader.setVoltage(leftOutput + leftFeedforward);
+ m_rightLeader.setVoltage(rightOutput + rightFeedforward);
}
/**
@@ -145,8 +142,8 @@
// simulated encoder and gyro. We negate the right side so that positive
// voltages make the right side move forward.
m_drivetrainSimulator.setInputs(
- m_leftGroup.get() * RobotController.getInputVoltage(),
- m_rightGroup.get() * RobotController.getInputVoltage());
+ m_leftLeader.get() * RobotController.getInputVoltage(),
+ m_rightLeader.get() * RobotController.getInputVoltage());
m_drivetrainSimulator.update(0.02);
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java
index d6ae048..0b791c5 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java
@@ -53,33 +53,17 @@
tab.add("Compressor", m_compressor);
// Also publish some raw data
- tab.addDouble(
- "PH Pressure [PSI]",
- () -> {
- // Get the pressure (in PSI) from the analog sensor connected to the PH.
- // This function is supported only on the PH!
- // On a PCM, this function will return 0.
- return m_compressor.getPressure();
- });
- tab.addDouble(
- "Compressor Current",
- () -> {
- // Get compressor current draw.
- return m_compressor.getCurrent();
- });
- tab.addBoolean(
- "Compressor Active",
- () -> {
- // Get whether the compressor is active.
- return m_compressor.isEnabled();
- });
- tab.addBoolean(
- "Pressure Switch",
- () -> {
- // Get the digital pressure switch connected to the PCM/PH.
- // The switch is open when the pressure is over ~120 PSI.
- return m_compressor.getPressureSwitchValue();
- });
+ // Get the pressure (in PSI) from the analog sensor connected to the PH.
+ // This function is supported only on the PH!
+ // On a PCM, this function will return 0.
+ tab.addDouble("PH Pressure [PSI]", m_compressor::getPressure);
+ // Get compressor current draw.
+ tab.addDouble("Compressor Current", m_compressor::getCurrent);
+ // Get whether the compressor is active.
+ tab.addBoolean("Compressor Active", m_compressor::isEnabled);
+ // Get the digital pressure switch connected to the PCM/PH.
+ // The switch is open when the pressure is over ~120 PSI.
+ tab.addBoolean("Pressure Switch", m_compressor::getPressureSwitchValue);
}
@SuppressWarnings("PMD.UnconditionalIfStatement")
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java
index 69997d1..3f91c93 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java
@@ -19,7 +19,6 @@
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
@@ -92,7 +91,7 @@
// An encoder set up to measure arm position in radians.
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
- private final MotorController m_motor = new PWMSparkMax(kMotorPort);
+ private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
// A joystick to read the trigger from.
private final Joystick m_joystick = new Joystick(kJoystickPort);
@@ -126,7 +125,7 @@
goal = new TrapezoidProfile.State(kLoweredPosition, 0.0);
}
// Step our TrapezoidalProfile forward 20ms and set it as our next reference
- m_lastProfiledReference = m_profile.calculate(0.020, goal, m_lastProfiledReference);
+ m_lastProfiledReference = m_profile.calculate(0.020, m_lastProfiledReference, goal);
m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);
// Correct our Kalman filter's state vector estimate with encoder data.
m_loop.correct(VecBuilder.fill(m_encoder.getDistance()));
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
index 406178a..79c871c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
@@ -9,13 +9,13 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
@@ -26,19 +26,16 @@
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
- private final MotorControllerGroup m_leftMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kLeftMotor1Port),
- new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+ private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+ private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
- private final MotorControllerGroup m_rightMotors =
- new MotorControllerGroup(
- new PWMSparkMax(DriveConstants.kRightMotor1Port),
- new PWMSparkMax(DriveConstants.kRightMotor2Port));
+ private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+ private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
- private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+ private final DifferentialDrive m_drive =
+ new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
@@ -70,10 +67,16 @@
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ SendableRegistry.addChild(m_drive, m_leftLeader);
+ SendableRegistry.addChild(m_drive, m_rightLeader);
+
+ m_leftLeader.addFollower(m_leftFollower);
+ m_rightLeader.addFollower(m_rightFollower);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
- m_rightMotors.setInverted(true);
+ m_rightLeader.setInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -131,8 +134,8 @@
// We negate the right side so that positive voltages make the right side
// move forward.
m_drivetrainSimulator.setInputs(
- m_leftMotors.get() * RobotController.getBatteryVoltage(),
- m_rightMotors.get() * RobotController.getBatteryVoltage());
+ m_leftLeader.get() * RobotController.getBatteryVoltage(),
+ m_rightLeader.get() * RobotController.getBatteryVoltage());
m_drivetrainSimulator.update(0.020);
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
@@ -202,8 +205,8 @@
* @param rightVolts the commanded right output
*/
public void tankDriveVolts(double leftVolts, double rightVolts) {
- m_leftMotors.setVoltage(leftVolts);
- m_rightMotors.setVoltage(rightVolts);
+ m_leftLeader.setVoltage(leftVolts);
+ m_rightLeader.setVoltage(rightVolts);
m_drive.feed();
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java
index 2069521..5294191 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java
@@ -19,7 +19,6 @@
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
@@ -96,7 +95,7 @@
// An encoder set up to measure elevator height in meters.
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
- private final MotorController m_motor = new PWMSparkMax(kMotorPort);
+ private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
// A joystick to read the trigger from.
private final Joystick m_joystick = new Joystick(kJoystickPort);
@@ -130,7 +129,7 @@
goal = new TrapezoidProfile.State(kLowGoalPosition, 0.0);
}
// Step our TrapezoidalProfile forward 20ms and set it as our next reference
- m_lastProfiledReference = m_profile.calculate(0.020, goal, m_lastProfiledReference);
+ m_lastProfiledReference = m_profile.calculate(0.020, m_lastProfiledReference, goal);
m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);
// Correct our Kalman filter's state vector estimate with encoder data.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java
index 06e8d29..94566ea 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java
@@ -17,7 +17,6 @@
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
@@ -77,7 +76,7 @@
// An encoder set up to measure flywheel velocity in radians per second.
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
- private final MotorController m_motor = new PWMSparkMax(kMotorPort);
+ private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
// A joystick to read the trigger from.
private final Joystick m_joystick = new Joystick(kJoystickPort);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java
index 26ee8f7..3a073fe 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java
@@ -16,7 +16,6 @@
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
@@ -72,7 +71,7 @@
// An encoder set up to measure flywheel velocity in radians per second.
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
- private final MotorController m_motor = new PWMSparkMax(kMotorPort);
+ private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
// A joystick to read the trigger from.
private final Joystick m_joystick = new Joystick(kJoystickPort);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
index 65089f9..c168860 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
@@ -12,7 +12,6 @@
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
public class SwerveModule {
@@ -23,8 +22,8 @@
private static final double kModuleMaxAngularAcceleration =
2 * Math.PI; // radians per second squared
- private final MotorController m_driveMotor;
- private final MotorController m_turningMotor;
+ private final PWMSparkMax m_driveMotor;
+ private final PWMSparkMax m_turningMotor;
private final Encoder m_driveEncoder;
private final Encoder m_turningEncoder;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
index cf0ff8c..1f8e3e5 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
@@ -19,6 +19,8 @@
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -82,7 +84,7 @@
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(DriveConstants.kDriveKinematics);
- // An example trajectory to follow. All units in meters.
+ // An example trajectory to follow. All units in meters.
Trajectory exampleTrajectory =
TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
@@ -111,10 +113,11 @@
m_robotDrive::setModuleStates,
m_robotDrive);
- // Reset odometry to the starting pose of the trajectory.
- m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
-
- // Run path following command, then stop at the end.
- return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
+ // Reset odometry to the initial pose of the trajectory, run path following
+ // command, then stop at the end.
+ return Commands.sequence(
+ new InstantCommand(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose())),
+ swerveControllerCommand,
+ new InstantCommand(() -> m_robotDrive.drive(0, 0, 0, false)));
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
index 923af5b..6eb80e0 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
@@ -71,7 +71,7 @@
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
- xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+ xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
periodSeconds));
SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
index 938b6c8..8ee8f4b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
@@ -12,7 +12,6 @@
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
public class SwerveModule {
@@ -23,8 +22,8 @@
private static final double kModuleMaxAngularAcceleration =
2 * Math.PI; // radians per second squared
- private final MotorController m_driveMotor;
- private final MotorController m_turningMotor;
+ private final PWMSparkMax m_driveMotor;
+ private final PWMSparkMax m_turningMotor;
private final Encoder m_driveEncoder;
private final Encoder m_turningEncoder;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java
new file mode 100644
index 0000000..285b98f
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java
@@ -0,0 +1,83 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.sysid;
+
+import edu.wpi.first.math.util.Units;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kLeftMotor1Port = 0;
+ public static final int kLeftMotor2Port = 1;
+ public static final int kRightMotor1Port = 2;
+ public static final int kRightMotor2Port = 3;
+
+ public static final int[] kLeftEncoderPorts = {0, 1};
+ public static final int[] kRightEncoderPorts = {2, 3};
+ public static final boolean kLeftEncoderReversed = false;
+ public static final boolean kRightEncoderReversed = true;
+
+ public static final int kEncoderCPR = 1024;
+ public static final double kWheelDiameterMeters = Units.inchesToMeters(6);
+ public static final double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
+ }
+
+ public static final class ShooterConstants {
+ public static final int[] kEncoderPorts = {4, 5};
+ public static final boolean kEncoderReversed = false;
+ public static final int kEncoderCPR = 1024;
+ public static final double kEncoderDistancePerPulse =
+ // Distance units will be rotations
+ 1.0 / (double) kEncoderCPR;
+
+ public static final int kShooterMotorPort = 4;
+ public static final int kFeederMotorPort = 5;
+
+ public static final double kShooterFreeRPS = 5300;
+ public static final double kShooterTargetRPS = 4000;
+ public static final double kShooterToleranceRPS = 50;
+
+ // These are not real PID gains, and will have to be tuned for your specific robot.
+ public static final double kP = 1;
+
+ // On a real robot the feedforward constants should be empirically determined; these are
+ // reasonable guesses.
+ public static final double kSVolts = 0.05;
+ public static final double kVVoltSecondsPerRotation =
+ // Should have value 12V at free speed...
+ 12.0 / kShooterFreeRPS;
+
+ public static final double kFeederSpeed = 0.5;
+ }
+
+ public static final class IntakeConstants {
+ public static final int kMotorPort = 6;
+ public static final int[] kSolenoidPorts = {2, 3};
+ }
+
+ public static final class StorageConstants {
+ public static final int kMotorPort = 7;
+ public static final int kBallSensorPort = 6;
+ }
+
+ public static final class AutoConstants {
+ public static final double kTimeoutSeconds = 3;
+ public static final double kDriveDistanceMeters = 2;
+ public static final double kDriveSpeed = 0.5;
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 0;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Main.java
new file mode 100644
index 0000000..1fdd4b7
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Main.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.sysid;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+ private Main() {}
+
+ /**
+ * Main initialization function. Do not perform any initialization here.
+ *
+ * <p>If you change your main robot class, change the parameter type.
+ */
+ public static void main(String... args) {
+ RobotBase.startRobot(Robot::new);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Robot.java
new file mode 100644
index 0000000..0b0f142
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Robot.java
@@ -0,0 +1,92 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.sysid;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private final SysIdRoutineBot m_robot = new SysIdRoutineBot();
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Configure default commands and condition bindings on robot startup
+ m_robot.configureBindings();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /** This function is called once each time the robot enters Disabled mode. */
+ @Override
+ public void disabledInit() {}
+
+ @Override
+ public void disabledPeriodic() {}
+
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robot.getAutonomousCommand();
+
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /** This function is called periodically during autonomous. */
+ @Override
+ public void autonomousPeriodic() {}
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /** This function is called periodically during operator control. */
+ @Override
+ public void teleopPeriodic() {}
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /** This function is called periodically during test mode. */
+ @Override
+ public void testPeriodic() {}
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java
new file mode 100644
index 0000000..6665f3f
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java
@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.sysid;
+
+import static edu.wpi.first.wpilibj.examples.sysid.Constants.OIConstants;
+
+import edu.wpi.first.wpilibj.examples.sysid.subsystems.Drive;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
+ */
+public class SysIdRoutineBot {
+ // The robot's subsystems
+ private final Drive m_drive = new Drive();
+
+ // The driver's controller
+ CommandXboxController m_driverController =
+ new CommandXboxController(OIConstants.kDriverControllerPort);
+
+ /**
+ * Use this method to define bindings between conditions and commands. These are useful for
+ * automating robot behaviors based on button and sensor input.
+ *
+ * <p>Should be called during {@link Robot#robotInit()}.
+ *
+ * <p>Event binding methods are available on the {@link Trigger} class.
+ */
+ public void configureBindings() {
+ // Control the drive with split-stick arcade controls
+ m_drive.setDefaultCommand(
+ m_drive.arcadeDriveCommand(
+ () -> -m_driverController.getLeftY(), () -> -m_driverController.getRightX()));
+
+ // Bind full set of SysId routine tests to buttons; a complete routine should run each of these
+ // once.
+ m_driverController.a().whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+ m_driverController.b().whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+ m_driverController.x().whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
+ m_driverController.y().whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+ }
+
+ /**
+ * Use this to define the command that runs during autonomous.
+ *
+ * <p>Scheduled during {@link Robot#autonomousInit()}.
+ */
+ public Command getAutonomousCommand() {
+ // Do nothing
+ return m_drive.run(() -> {});
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java
new file mode 100644
index 0000000..3c96108
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java
@@ -0,0 +1,132 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.sysid.subsystems;
+
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import java.util.function.DoubleSupplier;
+
+public class Drive extends SubsystemBase {
+ // The motors on the left side of the drive.
+ private final PWMSparkMax m_leftMotor = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+
+ // The motors on the right side of the drive.
+ private final PWMSparkMax m_rightMotor = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+
+ // The robot's drive
+ private final DifferentialDrive m_drive =
+ new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
+
+ // The left-side drive encoder
+ private final Encoder m_leftEncoder =
+ new Encoder(
+ DriveConstants.kLeftEncoderPorts[0],
+ DriveConstants.kLeftEncoderPorts[1],
+ DriveConstants.kLeftEncoderReversed);
+
+ // The right-side drive encoder
+ private final Encoder m_rightEncoder =
+ new Encoder(
+ DriveConstants.kRightEncoderPorts[0],
+ DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
+
+ // Mutable holder for unit-safe voltage values, persisted to avoid reallocation.
+ private final MutableMeasure<Voltage> m_appliedVoltage = mutable(Volts.of(0));
+ // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
+ private final MutableMeasure<Distance> m_distance = mutable(Meters.of(0));
+ // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
+ private final MutableMeasure<Velocity<Distance>> m_velocity = mutable(MetersPerSecond.of(0));
+
+ // Create a new SysId routine for characterizing the drive.
+ private final SysIdRoutine m_sysIdRoutine =
+ new SysIdRoutine(
+ // Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage.
+ new SysIdRoutine.Config(),
+ new SysIdRoutine.Mechanism(
+ // Tell SysId how to plumb the driving voltage to the motors.
+ (Measure<Voltage> volts) -> {
+ m_leftMotor.setVoltage(volts.in(Volts));
+ m_rightMotor.setVoltage(volts.in(Volts));
+ },
+ // Tell SysId how to record a frame of data for each motor on the mechanism being
+ // characterized.
+ log -> {
+ // Record a frame for the left motors. Since these share an encoder, we consider
+ // the entire group to be one motor.
+ log.motor("drive-left")
+ .voltage(
+ m_appliedVoltage.mut_replace(
+ m_leftMotor.get() * RobotController.getBatteryVoltage(), Volts))
+ .linearPosition(m_distance.mut_replace(m_leftEncoder.getDistance(), Meters))
+ .linearVelocity(
+ m_velocity.mut_replace(m_leftEncoder.getRate(), MetersPerSecond));
+ // Record a frame for the right motors. Since these share an encoder, we consider
+ // the entire group to be one motor.
+ log.motor("drive-right")
+ .voltage(
+ m_appliedVoltage.mut_replace(
+ m_rightMotor.get() * RobotController.getBatteryVoltage(), Volts))
+ .linearPosition(m_distance.mut_replace(m_rightEncoder.getDistance(), Meters))
+ .linearVelocity(
+ m_velocity.mut_replace(m_rightEncoder.getRate(), MetersPerSecond));
+ },
+ // Tell SysId to make generated commands require this subsystem, suffix test state in
+ // WPILog with this subsystem's name ("drive")
+ this));
+
+ /** Creates a new Drive subsystem. */
+ public Drive() {
+ // Add the second motors on each side of the drivetrain
+ m_leftMotor.addFollower(new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+ m_rightMotor.addFollower(new PWMSparkMax(DriveConstants.kRightMotor2Port));
+
+ // We need to invert one side of the drivetrain so that positive voltages
+ // result in both sides moving forward. Depending on how your robot's
+ // gearbox is constructed, you might have to invert the left side instead.
+ m_rightMotor.setInverted(true);
+
+ // Sets the distance per pulse for the encoders
+ m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ }
+
+ /**
+ * Returns a command that drives the robot with arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) {
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ return run(() -> m_drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble()))
+ .withName("arcadeDrive");
+ }
+
+ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+ return m_sysIdRoutine.quasistatic(direction);
+ }
+
+ public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+ return m_sysIdRoutine.dynamic(direction);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java
index 3883d14..a217f51 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java
@@ -4,10 +4,10 @@
package edu.wpi.first.wpilibj.examples.tankdrive;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
@@ -15,27 +15,30 @@
* the code necessary to operate a robot with tank drive.
*/
public class Robot extends TimedRobot {
- private DifferentialDrive m_myRobot;
+ private DifferentialDrive m_robotDrive;
private Joystick m_leftStick;
private Joystick m_rightStick;
- private final MotorController m_leftMotor = new PWMSparkMax(0);
- private final MotorController m_rightMotor = new PWMSparkMax(1);
+ private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
+ private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
@Override
public void robotInit() {
+ SendableRegistry.addChild(m_robotDrive, m_leftMotor);
+ SendableRegistry.addChild(m_robotDrive, m_rightMotor);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightMotor.setInverted(true);
- m_myRobot = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ m_robotDrive = new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
m_leftStick = new Joystick(0);
m_rightStick = new Joystick(1);
}
@Override
public void teleopPeriodic() {
- m_myRobot.tankDrive(-m_leftStick.getY(), -m_rightStick.getY());
+ m_robotDrive.tankDrive(-m_leftStick.getY(), -m_rightStick.getY());
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java
index c6a985a..c1a850c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java
@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.examples.tankdrivexboxcontroller;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -16,11 +17,15 @@
public class Robot extends TimedRobot {
private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
- private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final DifferentialDrive m_robotDrive =
+ new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
private final XboxController m_driverController = new XboxController(0);
@Override
public void robotInit() {
+ SendableRegistry.addChild(m_robotDrive, m_leftMotor);
+ SendableRegistry.addChild(m_robotDrive, m_rightMotor);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
index 532e9ac..33681d0 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
@@ -6,6 +6,7 @@
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.filter.MedianFilter;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Ultrasonic;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -21,8 +22,7 @@
static final double kHoldDistanceMillimeters = 1.0e3;
// proportional speed constant
- // negative because applying positive voltage will bring us closer to the target
- private static final double kP = -0.001;
+ private static final double kP = 0.001;
// integral speed constant
private static final double kI = 0.0;
// derivative speed constant
@@ -41,9 +41,15 @@
private final Ultrasonic m_ultrasonic = new Ultrasonic(kUltrasonicPingPort, kUltrasonicEchoPort);
private final PWMSparkMax m_leftMotor = new PWMSparkMax(kLeftMotorPort);
private final PWMSparkMax m_rightMotor = new PWMSparkMax(kRightMotorPort);
- private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final DifferentialDrive m_robotDrive =
+ new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
private final PIDController m_pidController = new PIDController(kP, kI, kD);
+ public Robot() {
+ SendableRegistry.addChild(m_robotDrive, m_leftMotor);
+ SendableRegistry.addChild(m_robotDrive, m_rightMotor);
+ }
+
@Override
public void autonomousInit() {
// Set setpoint of the pid controller
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/unittest/subsystems/Intake.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/unittest/subsystems/Intake.java
index a51d505..e84a607 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/unittest/subsystems/Intake.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/unittest/subsystems/Intake.java
@@ -44,7 +44,7 @@
}
@Override
- public void close() throws Exception {
+ public void close() {
m_piston.close();
m_motor.close();
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/Drivetrain.java
index f618add..87b985d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/Drivetrain.java
@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.examples.xrpreference.subsystems;
+import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.BuiltInAccelerometer;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -29,7 +30,8 @@
private final Encoder m_rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
- private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final DifferentialDrive m_diffDrive =
+ new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
// Set up the XRPGyro
private final XRPGyro m_gyro = new XRPGyro();
@@ -39,6 +41,9 @@
/** Creates a new Drivetrain. */
public Drivetrain() {
+ SendableRegistry.addChild(m_diffDrive, m_leftMotor);
+ SendableRegistry.addChild(m_diffDrive, m_rightMotor);
+
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/subsystems/RomiDrivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/subsystems/RomiDrivetrain.java
index 276e5ce..c1cfac1 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/subsystems/RomiDrivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/subsystems/RomiDrivetrain.java
@@ -24,7 +24,8 @@
private final Encoder m_rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
- private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final DifferentialDrive m_diffDrive =
+ new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
/** Creates a new RomiDrivetrain. */
public RomiDrivetrain() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/RomiDrivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/RomiDrivetrain.java
index 4b73a44..7d2a438 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/RomiDrivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/RomiDrivetrain.java
@@ -23,7 +23,8 @@
private final Encoder m_rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
- private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final DifferentialDrive m_diffDrive =
+ new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
/** Creates a new RomiDrivetrain. */
public RomiDrivetrain() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/RomiDrivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/RomiDrivetrain.java
index e4f8559..a0f4f47 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/RomiDrivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/RomiDrivetrain.java
@@ -23,7 +23,8 @@
private final Encoder m_rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
- private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final DifferentialDrive m_diffDrive =
+ new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
/** Creates a new RomiDrivetrain. */
public RomiDrivetrain() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/subsystems/XRPDrivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/subsystems/XRPDrivetrain.java
index a3a1063..85febbe 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/subsystems/XRPDrivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/subsystems/XRPDrivetrain.java
@@ -27,7 +27,8 @@
private final Encoder m_rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
- private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final DifferentialDrive m_diffDrive =
+ new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
/** Creates a new XRPDrivetrain. */
public XRPDrivetrain() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/XRPDrivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/XRPDrivetrain.java
index 8f73bae..a245955 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/XRPDrivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/XRPDrivetrain.java
@@ -26,7 +26,8 @@
private final Encoder m_rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
- private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final DifferentialDrive m_diffDrive =
+ new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
/** Creates a new XRPDrivetrain. */
public XRPDrivetrain() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/XRPDrivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/XRPDrivetrain.java
index e66d750..790acc7 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/XRPDrivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/XRPDrivetrain.java
@@ -26,7 +26,8 @@
private final Encoder m_rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
- private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final DifferentialDrive m_diffDrive =
+ new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
/** Creates a new XRPDrivetrain. */
public XRPDrivetrain() {
diff --git a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java
index 342daa2..f0a27b8 100644
--- a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java
+++ b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java
@@ -80,7 +80,7 @@
m_driveSim.update(0.02);
double startingDistance = m_startToObject;
- double range = startingDistance - m_driveSim.getLeftPositionMeters();
+ double range = m_driveSim.getLeftPositionMeters() - startingDistance;
m_ultrasonicSim.setRangeMeters(range);
m_distanceMM = range * 1.0e3;
@@ -126,10 +126,9 @@
}
{
- // advance 100 timesteps
- SimHooks.stepTiming(2.0);
+ SimHooks.stepTiming(5.0);
- assertEquals(Robot.kHoldDistanceMillimeters, m_distanceMM, 10);
+ assertEquals(Robot.kHoldDistanceMillimeters, m_distanceMM, 10.0);
}
}
}