Squashed 'third_party/allwpilib/' changes from 66b57f032..e473a00f9
e473a00f9 [wpiutil] Base64: Add unsigned span/vector variants (#3702)
52f2d580e [wpiutil] raw_uv_ostream: Add reset() (#3701)
d7b1e3576 [wpiutil] WebSocket: move std::function (#3700)
93799fbe9 [examples] Fix description of TrapezoidProfileSubsystem (#3699)
b84644740 [wpimath] Document pose estimator states, inputs, and outputs (#3698)
2dc35c139 [wpimath] Fix classpaths for JNI class loads (#3697)
2cb171f6f [docs] Set Doxygen extract_all to true and fix Doxygen failures (#3695)
a939cd9c8 [wpimath] Print uncontrollable/unobservable models in LQR and KF (#3694)
d5270d113 [wpimath] Clean up C++ StateSpaceUtil tests (#3692)
b20903960 [wpimath] Remove redundant discretization tests from StateSpaceUtilTest (#3689)
c0cb545b4 [wpilibc] Add deprecated Doxygen attribute to SpeedController (#3691)
35c9f66a7 [wpilib] Rename PneumaticsHub to PneumaticHub (#3686)
796d03d10 [wpiutil] Remove unused LLVM header (#3688)
8723caf78 [wpilibj] Make Java TrapezoidProfile.Constraints an immutable class (#3687)
187f50a34 [wpimath] Catch incorrect parameters to state-space models earlier (#3680)
8d04606c4 Replace instances of frc-characterization with SysId (NFC) (#3681)
b82d4f6e5 [hal, cscore, ntcore] Use WPI common handle type base
87e34967e [wpiutil] Add synchronization primitives
e32499c54 [wpiutil] Add ParallelTcpConnector (#3655)
aa0b49228 [wpilib] Remove redundant "quick turn" docs for curvature drive (NFC) (#3674)
57301a7f9 [hal] REVPH: Start closed-loop compressor control on init (#3673)
d1842ea8f [wpilib] Improve interrupt docs (NFC) (#3679)
558151061 [wpiutil] Add DsClient (#3654)
181723e57 Replace `.to<double>()` and `.template to<double>()` with `.value()` (#3667)
6bc1db44b [commands] Add pointer overload of AddRequirements (artf6003) (#3669)
737b57ed5 [wpimath] Update to drake v0.35.0 (#3665)
4d287d1ae [build] Upgrade WPIJREArtifact to JRE 2022-11.0.12u5 (#3666)
f26eb5ada [hal] Fix another typo (get -> gets) (NFC) (#3663)
94ed275ba [hal] Fix misspelling (numer -> number) (NFC) (#3662)
ac2f44da3 [wpiutil] uv: use move for std::function (#3653)
75fa1fbfb [wpiutil] json::serializer: Optimize construction (#3647)
5e689faea [wpiutil] Import MessagePack implementation (mpack) (#3650)
649a50b40 [wpiutil] Add LEB128 byte-by-byte reader (#3652)
e94397a97 [wpiutil] Move json_serializer.h to public headers (#3646)
4ec58724d [wpiutil] uv::Tcp: Clarify SetNoDelay documentation (#3649)
8cb294aa4 [wpiutil] WebSocket: Make Shutdown() public (#3651)
2b3a9a52b [wpiutil] json: Fix map iterator key() for std::string_view (#3645)
138cbb94b [wpiutil] uv::Async: Add direct call for no-parameter specialization (#3648)
e56d6dea8 [ci] Update testbench pool image to ubuntu-latest (#3643)
43f30e44e [build] Enable comments in doxygen source files (#3644)
9e6db17ef [build] Enable doxygen preprocessor expansion of WPI_DEPRECATED (#3642)
0e631ad2f Add WPILib version to issue template (#3641)
6229d8d2f [build] Docs: set case_sense_names to false (#3392)
4647d09b5 [docs] Fix Doxygen warnings, add CI docs lint job (#3639)
4ad3a5402 [hal] Fix PWM allocation channel (#3637)
05e5feac4 [docs] Fix brownout docs (NFC) (#3638)
67df469c5 [examples] Remove old command-based templates and examples (#3263)
689e9ccfb [hal, wpilib] Add brownout voltage configuration (#3632)
9cd4bc407 [docs] Add useLocal to avoid using installer artifacts (#3634)
61996c2bb [cscore] Fix Java direct callback notifications (#3631)
6d3dd99eb [build] Update to newest native-utils (#3633)
f0b484892 [wpiutil] Fix StringMap iterator equality check (#3629)
8352cbb7a Update development build instructions for 2022 (#3616)
6da08b71d [examples] Fix Intermediate Vision Java Example description (#3628)
5d99059bf [wpiutil] Remove optional.h (#3627)
fa41b106a [glass, wpiutil] Add missing format args (#3626)
4e3fd7d42 [build] Enable Zc:__cplusplus for Windows (#3625)
791d8354d [build] Suppress deprecation/removal warnings for old commands (#3618)
10f19e6fc [hal, wpilib] Add REV PneumaticsHub (#3600)
4c61a1305 [ntcore] Revert to per-element copy for toNative() (#3621)
7b3f62244 [wpiutil] SendableRegistry: Print exception stacktrace (#3620)
d347928e4 [hal] Use better error for when console out is enabled while attempting to use onboard serial port (#3622)
cc31079a1 [hal] Use setcap instead of setuid for setting thread priorities (#3613)
4676648b7 [wpimath] Upgrade to Drake v0.34.0 (#3607)
c7594c911 [build] Allow building wpilibc in cmake without cscore and opencv (#3605)
173cb7359 [wpilib] Add TimesliceRobot (#3502)
af295879f [hal] Set error status for I2C port out of range (#3603)
95dd20a15 [build] Enable spotbugs (#3601)
b65fce86b [wpilib] Remove Timer lock in wpilibj and update docs (#3602)
3b8d3bbcb Remove unused and add missing deprecated.h includes (#3599)
f9e976467 [examples] Rename DriveTrain classes to Drivetrain (#3594)
118a27be2 [wpilib] Add Timer tests (#3595)
59c89428e [wpilib] Deprecate Timer::HasPeriodPassed() (#3598)
202ca5e78 Force C++17 in .clang-format (#3597)
d6f185d8e Rename tests for consistency (#3592)
54ca474db [ci] Enable asan and tsan in CI for tests that pass (#3591)
1ca383b23 Add Debouncer (#3590)
179fde3a7 [build] Update to 2022 native utils and gradle 7 (#3588)
50198ffcf [examples] Add Mechanism2d visualization to Elevator Sim (#3587)
a446c2559 [examples] Synchronize C++ and Java Mechanism2d examples (#3589)
a7fb83103 [ci] clang-tidy: Generate compilation commands DB with Gradle (#3585)
4f5e0c9f8 [examples] Update ArmSimulation example to use Mechanism2d (#3572)
8164b91dc [CI] Print CMake test output on failure (#3583)
4d5fca27e [wpilib] Impove Mechanism2D documentation (NFC) (#3584)
fe59e4b9f Make C++ test names more consistent (#3586)
5c8868549 [wpilibc] Fix C++ MechanisimRoot2D to use same NT entries as Java/Glass (#3582)
9359431ba [wpimath] Clean up Eigen usage
72716f51c [wpimath] Upgrade to Eigen 3.4
382deef75 [wpimath] Explicitly export wpimath symbols
161e21173 [ntcore] Match standard handle layout, only allow 16 instances (#3577)
263a24811 [wpimath] Use jinja for codegen (#3574)
725251d29 [wpilib] Increase tolerances of DifferentialDriveSimTest (#3581)
4dff87301 [wpimath] Make LinearFilter::Factorial() constexpr (#3579)
60ede67ab [hal, wpilib] Switch PCM to be a single object that is allowed to be duplicated (#3475)
906bfc846 [build] Add CMake build support for sanitizers (#3576)
0d4f08ad9 [hal] Simplify string copy of joystick name (#3575)
a52bf87b7 [wpiutil] Add Java function package (#3570)
40c7645d6 [wpiutil] UidVector: Return old object from erase() (#3571)
5b886a23f [wpiutil] jni_util: Add size, operator[] to JArrayRef (#3569)
65797caa7 [sim] Fix halsim_ds_socket stringop overflow warning from GCC 10 (#3568)
66abb3988 [hal] Update runtime enum to allow selecting roborio 2 (#3565)
95a12e0ee [hal] UidSetter: Don't revert euid if its already current value (#3566)
27951442b [wpimath] Use external Eigen headers only (#3564)
c42e053ae [docs] Update to doxygen 1.9.2 (#3562)
e7048c8c8 [docs] Disable doxygen linking for common words that are also classes (#3563)
d8e0b6c97 [wpilibj] Fix java async interrupts (#3559)
5e6c34c61 Update to 2022 roborio image (#3537)
828f073eb [wpiutil] Fix uv::Buffer memory leaks caught by asan (#3555)
2dd5701ac [cscore] Fix mutex use-after-free in cscore test (#3557)
531439198 [ntcore] Fix NetworkTables memory leaks caught by asan (#3556)
3d9a4d585 [wpilibc] Fix AnalogTriggerOutput memory leak reported by asan (#3554)
54eda5928 [wpiutil] Ignore ubsan vptr upcast warning in SendableHelper moves (#3553)
5a4f75c9f [wpilib] Replace Speed controller comments with motor controller (NFC) (#3551)
7810f665f [wpiutil] Fix bug in uleb128 (#3540)
697e2dd33 [wpilib] Fix errant jaguar reference in comments (NFC) (#3550)
936c64ff5 [docs] Enable -linksource for javadocs (#3549)
1ea654954 [build] Upgrade CMake build to googletest 1.11.0 (#3548)
32d9949e4 [wpimath] Move controller tests to wpimath (#3541)
01ba56a8a [hal] Replace strncpy with memcpy (#3539)
e109c4251 [build] Rename makeSim flag to forceGazebo to better describe what it does (#3535)
e4c709164 [docs] Use a doxygen theme and add logo (#3533)
960b6e589 [wpimath] Fix Javadoc warning (#3532)
82eef8d5e [hal] Remove over current fault HAL functions from REV PDH (#3526)
aa3848b2c [wpimath] Move RobotDriveBase::ApplyDeadband() to MathUtil (#3529)
3b5d0d141 [wpimath] Add LinearFilter::BackwardFiniteDifference() (#3528)
c8fc715fe [wpimath] Upgrade drake files to v0.33.0 (#3531)
e5fe3a8e1 [build] Treat javadoc warnings as errors in CI and fix warnings (#3530)
e0c6cd3dc [wpimath] Add an operator for composing two Transform2ds (#3527)
2edd510ab [sim] Add sim wrappers for sensors that use SimDevice (#3517)
2b3e2ebc1 [hal] Fix HAL Notifier thread priority setting (#3522)
ab4cb5932 [gitignore] Update gitignore to ignore bazel / clion files (#3524)
57c8615af [build] Generate spotless patch on failure (#3523)
b90317321 Replace std::cout and std::cerr with fmt::print() (#3519)
10cc8b89c [hal] [wpilib] Add initial support for the REV PDH (#3503)
5d9ae3cdb [hal] Set HAL Notifier thread as RT by default (#3482)
192d251ee [wpilibcIntegrationTests] Properly disable DMA integration tests (#3514)
031962608 [wpilib] Add PS4Controller, remove Hand from GenericHID/XboxController (#3345)
25f6f478a [wpilib] Rename DriverStation::IsOperatorControl() to IsTeleop() (#3505)
e80f09f84 [wpilibj] Add unit tests (#3501)
c159f91f0 [wpilib] Only read DS control word once in IterativeRobotBase (#3504)
eb790a74d Add rio development docs documenting myRobot deploy tasks (#3508)
e47451f5a [wpimath] Replace auto with Eigen types (#3511)
252b8c83b Remove Java formatting from build task in CI (#3507)
09666ff29 Shorten Gazebo CI build (#3506)
baf2e501d Update myRobot to use 2021 java (#3509)
5ac60f0a2 [wpilib] Remove IterativeRobotBase mode init prints (#3500)
fb2ee8ec3 [wpilib] Add TimedRobot functions for running code on mode exit (#3499)
94e0db796 [wpilibc] Add more unit tests (#3494)
b25324695 [wpilibj] Add units to parameter names (NFC) (#3497)
1ac73a247 [hal] Rename PowerDistributionPanel to PowerDistribution (#3466)
2014115bc [examples] frisbeebot: Fix typo and reflow comments (NFC) (#3498)
4a944dc39 [examples] Consistently use 0 for controller port (#3496)
3838cc4ec Use unicode characters in docs equations (#3487)
85748f2e6 [examples] Add C++ TankDrive example (#3493)
d7b8aa56d [wpilibj] Rename DriverStation In[Mode] functions to follow style guide (#3488)
16e096cf8 [build] Fix CMake Windows CI (#3490)
50af74c38 [wpimath] Clean up NumericalIntegration and add Discretization tests (#3489)
bfc209b12 Automate fmt update (#3486)
e7f9331e4 [build] Update to Doxygen 1.9.1 (#3008)
ab8e8aa2a [wpimath] Update drake with upstream (#3484)
1ef826d1d [wpimath] Fix IOException path in WPIMath JNI (#3485)
52bddaa97 [wpimath] Disable iostream support for units and enable fmtlib (#3481)
e4dc3908b [wpiutil] Upgrade to fmtlib 8.0.1 (#3483)
1daadb812 [wpimath] Implement Dormand-Prince integration method (#3476)
9c2723391 [cscore] Add [[nodiscard]] to GrabFrame functions (#3479)
7a8796414 [wpilib] Add Notifier integration tests (#3480)
f8f13c536 [wpilibcExamples] Prefix decimal numbers with 0 (#3478)
1adb69c0f [ntcore] Use "NetworkTables" instead of "Network Tables" in NT specs (#3477)
5f5830b96 Upload wpiformat diff if one exists (#3474)
9fb4f35bb [wpimath] Add tests for DARE overload with Q, R, and N matrices (#3472)
c002e6f92 Run wpiformat (#3473)
c154e5262 [wpilib] Make solenoids exclusive use, PCM act like old sendable compressor (#3464)
6ddef1cca [hal] JNI setDIO: use a boolean and not a short (#3469)
9d68d9582 Remove extra newlines after open curly braces (NFC) (#3471)
a4233e1a1 [wpimath] Add script for updating Drake (#3470)
39373c6d2 Update README.md for new GCC version requirement (#3467)
d29acc90a [wpigui] Add option to reset UI on exit (#3463)
a371235b0 [ntcore] Fix dangling pointer in logger (#3465)
53b4891a5 [wpilibcintegrationtests] Fix deprecated Preferences usage (#3461)
646ded912 [wpimath] Remove incorrect discretization in pose estimators (#3460)
ea0b8f48e Fix some deprecation warnings due to fmtlib upgrade (#3459)
2067d7e30 [wpilibjexamples] Add wpimathjni, wpiutiljni to library path (#3455)
866571ab4 [wpiutil] Upgrade to fmtlib 8.0.0 (#3457)
4e1fa0308 [build] Skip PDB copy on windows build servers (#3458)
b45572167 [build] Change CI back to 18.04 docker images (#3456)
57a160f1b [wpilibc] Fix LiveWindow deprecation warning in RobotBase skeleton template (#3454)
29ae8640d [HLT] Implement duty cycle cross connect tests (#3453)
ee6377e54 [HLT] Add relay and analog cross connects (#3452)
b0f1ae7ea [build] CMake: Build the HAL even if WITH_CSCORE=OFF (#3449)
7aae2b72d Replace std::to_string() with fmt::format() (#3451)
73fcbbd74 [HLT] Add relay digital cross connect tests (#3450)
e7bedde83 [HLT] Add PWM tests that use DMA as the back end (#3447)
7253edb1e [wpilibc] Timer: Fix deprecated warning (#3446)
efa28125c [wpilibc] Add message to RobotBase on how to read stacktrace (#3444)
9832fcfe1 [hal] Fix DIO direction getter (#3445)
49c71f9f2 [wpilibj] Clarify robot quit message (#3364)
791770cf6 [wpimath] Move controller from wpilibj to wpimath (#3439)
9ce9188ff [wpimath] Add ReportWarning to MathShared (#3441)
362066a9b [wpilib] Deprecate getInstance() in favor of static functions (#3440)
26ff9371d Initial commit of cross connect integration test project (#3434)
4a36f86c8 [hal] Add support for DMA to Java (#3158)
85144e47f [commands] Unbreak build (#3438)
b417d961e Split Sendable into NT and non-NT portions (#3432)
ef4ea84cb [commands] Change grouping decorator impl to flatten nested group structures (#3335)
b422665a3 [examples] Invert right side of drive subsystems (#3437)
186dadf14 [hal] Error if attempting to set DIO output on an input port (#3436)
04e64db94 Remove redundant C++ lambda parentheses (NFC) (#3433)
f60994ad2 [wpiutil] Rename Java package to edu.wpi.first.util (#3431)
cfa1ca96f [wpilibc] Make ShuffleboardValue non-copyable (#3430)
4d9ff7643 Fix documentation warnings generated by JavaDoc (NFC) (#3428)
9e1b7e046 [build] Fix clang-tidy and clang-format (#3429)
a77c6ff3a [build] Upgrade clang-format and clang-tidy (NFC) (#3422)
099fde97d [wpilib] Improve PDP comments (NFC) (#3427)
f8fc2463e [wpilibc, wpiutil] Clean up includes (NFC) (#3426)
e246b7884 [wpimath] Clean up member initialization in feedforward classes (#3425)
c1e128bd5 Disable frivolous PMD warnings and enable PMD in ntcore (#3419)
8284075ee Run "Lint and Format" CI job on push as well as pull request (#3412)
f7db09a12 [wpimath] Move C++ filters into filter folder to match Java (#3417)
f9c3d54bd [wpimath] Reset error covariance in pose estimator ResetPosition() (#3418)
0773f4033 [hal] Ensure HAL status variables are initialized to zero (#3421)
d068fb321 [build] Upgrade CI to use 20.04 docker images (#3420)
8d054c940 [wpiutil] Remove STLExtras.h
80f1d7921 [wpiutil] Split function_ref to a separate header
64f541325 Use wpi::span instead of wpi::ArrayRef across all libraries (#3414)
2abbbd9e7 [build] clang-tidy: Remove bugprone-exception-escape (#3415)
a5c471af7 [wpimath] Add LQR template specialization for 2x2 system
edd2f0232 [wpimath] Add DARE solver for Q, R, and N with LQR ctor overloads
b2c3b2dd8 Use std::string_view and fmtlib across all libraries (#3402)
4f1cecb8e [wpiutil] Remove Path.h (#3413)
b336eac34 [build] Publish halsim_ws_core to Maven
2a09f6fa4 [build] Also build sim modules as static libraries
0e702eb79 [hal] Add a unified PCM object (#3331)
dea841103 [wpimath] Add fmtlib formatter overloads for Eigen::Matrix and units (#3409)
82856cf81 [wpiutil] Improve wpi::circular_buffer iterators (#3410)
8aecda03e [wpilib] Fix a documentation typo (#3408)
5c817082a [wpilib] Remove InterruptableSensorBase and replace with interrupt classes (#2410)
15c521a7f [wpimath] Fix drivetrain system identification (#3406)
989de4a1b [build] Force all linker warnings to be fatal for rio builds (#3407)
d9eeb45b0 [wpilibc] Add units to Ultrasonic class API (#3403)
fe570e000 [wpiutil] Replace llvm filesystem with C++17 filesystem (#3401)
01dc0249d [wpimath] Move SlewRateLimiter from wpilib to wpimath (#3399)
93523d572 [wpilibc] Clean up integration tests (#3400)
4f7a4464d [wpiutil] Rewrite StringExtras for std::string_view (#3394)
e09293a15 [wpilibc] Transition C++ classes to units::second_t (#3396)
827b17a52 [build] Create run tasks for Glass and OutlineViewer (#3397)
a61037996 [wpiutil] Avoid MSVC warning on span include (#3393)
4e2c3051b [wpilibc] Use std::string_view instead of Twine (#3380)
50915cb7e [wpilibc] MotorSafety::GetDescription(): Return std::string (#3390)
f4e2d26d5 [wpilibc] Move NullDeleter from frc/Base.h to wpi/NullDeleter.h (#3387)
cb0051ae6 [wpilibc] SimDeviceSim: use fmtlib (#3389)
a238cec12 [wpiutil] Deprecate wpi::math constants in favor of wpi::numbers (#3383)
393bf23c0 [ntcore, cscore, wpiutil] Standardize template impl files on .inc extension (NFC) (#3124)
e7d9ba135 [sim] Disable flaky web server integration tests (#3388)
0a0003c11 [wpilibjExamples] Fix name of Java swerve drive pose estimator example (#3382)
7e1b27554 [wpilibc] Use default copies and moves when possible (#3381)
fb2a56e2d [wpilibc] Remove START_ROBOT_CLASS macro (#3384)
84218bfb4 [wpilibc] Remove frc namespace shim (#3385)
dd7824340 [wpilibc] Remove C++ compiler version static asserts (#3386)
484cf9c0e [wpimath] Suppress the -Wmaybe-uninitialized warning in Eigen (#3378)
a04d1b4f9 [wpilibc] DriverStation: Remove ReportError and ReportWarning
831c10bdf [wpilibc] Errors: Use fmtlib
87603e400 [wpiutil] Import fmtlib (#3375)
442621672 [wpiutil] Add ArrayRef/std::span/wpi::span implicit conversions
bc15b953b [wpiutil] Add std::span implementation
6d20b1204 [wpiutil] StringRef, Twine, raw_ostream: Add std::string_view support (#3373)
2385c2a43 [wpilibc] Remove Utility.h (#3376)
87384ea68 [wpilib] Fix PIDController continuous range error calculations (#3170)
04dae799a [wpimath] Add SimpleMotorFeedforward::Calculate(velocity, nextVelocity) overload (#3183)
0768c3903 [wpilib] DifferentialDrive: Remove right side inversion (#3340)
8dd8d4d2d [wpimath] Fix redundant nested math package introduced by #3316 (#3368)
49b06beed [examples] Add Field2d to RamseteController example (#3371)
4c562a445 [wpimath] Fix typo in comment of update_eigen.py (#3369)
fdbbf1188 [wpimath] Add script for updating Eigen
f1e64b349 [wpimath] Move Eigen unsupported folder into eigeninclude
224f3a05c [sim] Fix build error when building with GCC 11.1 (#3361)
ff56d6861 [wpilibj] Fix SpeedController deprecated warnings (#3360)
1873fbefb [examples] Fix Swerve and Mecanum examples (#3359)
80b479e50 [examples] Fix SwerveBot example to use unique encoder ports (#3358)
1f7c9adee [wpilibjExamples] Fix pose estimator examples (#3356)
9ebc3b058 [outlineviewer] Change default size to 600x400 (#3353)
e21b443a4 [build] Gradle: Make C++ examples runnable (#3348)
da590120c [wpilibj] Add MotorController.setVoltage default (#3347)
561d53885 [build] Update opencv to 4.5.2, imgui/implot to latest (#3344)
44ad67ca8 [wpilibj] Preferences: Add missing Deprecated annotation (#3343)
3fe8fc75a [wpilibc] Revert "Return reference from GetInstance" (#3342)
3cc2da332 Merge branch '2022'
a3cd90dd7 [wpimath] Fix classpath used by generate_numbers.py (#3339)
d6cfdd3ba [wpilib] Preferences: Deprecate Put* in favor of Set* (#3337)
ba08baabb [wpimath] Update Drake DARE solver to v0.29.0 (#3336)
497b712f6 [wpilib] Make IterativeRobotBase::m_period private with getter
f00dfed7a [wpilib] Remove IterativeRobot base class
3c0846168 [hal] Use last error reporting instead of PARAMETER_OUT_OF_RANGE (#3328)
5ef2b4fdc [wpilibj] Fix @deprecated warning for SerialPort constructor (#3329)
23d2326d1 [hal] Report previous allocation location for indexed resource duplicates (#3322)
e338f9f19 [build] Fix wpilibc runCpp task (#3327)
c8ff626fe [wpimath] Move Java classes to edu.wpi.first.math (#3316)
4e424d51f [wpilibj] DifferentialDrivetrainSim: Rename constants to match the style guide (#3312)
6b50323b0 [cscore] Use Lock2DSize if possible for Windows USB cameras (#3326)
65c148536 [wpilibc] Fix "control reaches end of non-void function" warning (#3324)
f99f62bee [wpiutil] uv Handle: Use malloc/free instead of new/delete (#3325)
365f5449c [wpimath] Fix MecanumDriveKinematics (#3266)
ff52f207c [glass, wpilib] Rewrite Mechanism2d (#3281)
ee0eed143 [wpimath] Add DCMotor factory function for Romi motors (#3319)
512738072 [hal] Add HAL_GetLastError to enable better error messages from HAL calls (#3320)
ced654880 [glass, outlineviewer] Update Mac icons to macOS 11 style (#3313)
936d3b9f8 [templates] Add Java template for educational robot (#3309)
6e31230ad [examples] Fix odometry update in SwerveControllerCommand example (#3310)
05ebe9318 Merge branch 'main' into 2022
aaf24e255 [wpilib] Fix initial heading behavior in HolonomicDriveController (#3290)
8d961dfd2 [wpilibc] Remove ErrorBase (#3306)
659b37ef9 [wpiutil] StackTrace: Include offset on Linux (#3305)
0abf6c904 [wpilib] Move motor controllers to motorcontrol package (#3302)
4630191fa [wpiutil] circular_buffer: Use value initialization instead of passing zero (#3303)
b7b178f49 [wpilib] Remove Potentiometer interface
687066af3 [wpilib] Remove GyroBase
6b168ab0c [wpilib] Remove PIDController, PIDOutput, PIDSource
948625de9 [wpimath] Document conversion from filter cutoff frequency to time constant (#3299)
3848eb8b1 [wpilibc] Fix flywhel -> flywheel typo in FlywheelSim (#3298)
3abe0b9d4 [cscore] Move java package to edu.wpi.first.cscore (#3294)
d7fabe81f [wpilib] Remove RobotDrive (#3295)
1dc81669c [wpilib] Remove GearTooth (#3293)
01d0e1260 [wpilib] Revert move of RomiGyro into main wpilibc/j (#3296)
397e569aa [ntcore] Remove "using wpi" from nt namespace
79267f9e6 [ntcore] Remove NetworkTable -> nt::NetworkTable shim
48ebe5736 [ntcore] Remove deprecated Java interfaces and classes
c2064c78b [ntcore] Remove deprecated ITable interfaces
36608a283 [ntcore] Remove deprecated C++ APIs
a1c87e1e1 [glass] LogView: Add "copy to clipboard" button (#3274)
fa7240a50 [wpimath] Fix typo in quintic spline basis matrix
ffb4d38e2 [wpimath] Add derivation for spline basis matrices
f57c188f2 [wpilib] Add AnalogEncoder(int) ctor (#3273)
8471c4fb2 [wpilib] FieldObject2d: Add setTrajectory() method (#3277)
c97acd18e [glass] Field2d enhancements (#3234)
ffb590bfc [wpilib] Fix Compressor sendable properties (#3269)
6137f98eb [hal] Rename SimValueCallback2 to SimValueCallback (#3212)
a6f653969 [hal] Move registerSimPeriodic functions to HAL package (#3211)
10c038d9b [glass] Plot: Fix window creation after removal (#3264)
2d2eaa3ef [wpigui] Ensure window will be initially visible (#3256)
4d28b1f0c [wpimath] Use JNI for trajectory serialization (#3257)
3de800a60 [wpimath] TrajectoryUtil.h: Comment formatting (NFC) (#3262)
eff592377 [glass] Plot: Don't overwrite series ID (#3260)
a79faace1 [wpilibc] Return reference from GetInstance (#3247)
9550777b9 [wpilib] PWMSpeedController: Use PWM by composition (#3248)
c8521a3c3 [glass] Plot: Set reasonable default window size (#3261)
d71eb2cf3 [glass] Plot: Show full source name as tooltip and in popup (#3255)
160fb740f [hal] Use std::lround() instead of adding 0.5 and truncating (#3012)
48e9f3951 [wpilibj] Remove wpilibj package CameraServer (#3213)
8afa596fd [wpilib] Remove deprecated Sendable functions and SendableBase (#3210)
d3e45c297 [wpimath] Make C++ geometry classes immutable (#3249)
2c98939c1 [glass] StringChooser: Don't call SameLine() at end
a18a7409f [glass] NTStringChooser: Clear value of deleted entries
2f19cf452 [glass] NetworkTablesHelper: listen to delete events
da96707dc Merge branch 'main' into 2022
c3a8bdc24 [build] Fix clang-tidy action (#3246)
21624ef27 Add ImGui OutlineViewer (#3220)
1032c9b91 [wpiutil] Unbreak wpi::Format on Windows (#3242)
2e07902d7 [glass] NTField2D: Fix name lookup (#3233)
6e23e1840 [wpilibc] Remove WPILib.h (#3235)
3e22e4506 [wpilib] Make KoP drivetrain simulation weight 60 lbs (#3228)
79d1bd6c8 [glass] NetworkTablesSetting: Allow disable of server option (#3227)
fe341a16f [examples] Use more logical elevator setpoints in GearsBot (#3198)
62abf46b3 [glass] NetworkTablesSettings: Don't block GUI (#3226)
a95a5e0d9 [glass] Move NetworkTablesSettings to libglassnt (#3224)
d6f6ceaba [build] Run Spotless formatter (NFC) (#3221)
0922f8af5 [commands] CommandScheduler.requiring(): Note return can be null (NFC) (#2934)
6812302ff [examples] Make DriveDistanceOffboard example work in sim (#3199)
f3f86b8e7 [wpimath] Add pose estimator overload for vision + std dev measurement (#3200)
1a2680b9e [wpilibj] Change CommandBase.withName() to return CommandBase (#3209)
435bbb6a8 [command] RamseteCommand: Output 0 if interrupted (#3216)
3cf44e0a5 [hal] Add function for changing HAL Notifier thread priority (#3218)
40b367513 [wpimath] Units.java: Add kg-lb conversions (#3203)
9f563d584 [glass] NT: Fix return value in StringToDoubleArray (#3208)
af4adf537 [glass] Auto-size plots to fit window (#3193)
2560146da [sim] GUI: Add option to show prefix in Other Devices (#3186)
eae3a6397 gitignore: Ignore .cache directory (#3196)
959611420 [wpilib] Require non-zero positive value for PIDController.period (#3175)
9522f2e8c [wpimath] Add methods to concatenate trajectories (#3139)
e42a0b6cf [wpimath] Rotation2d comment formatting (NFC) (#3162)
d1c7032de [wpimath] Fix order of setting gyro offset in pose estimators (#3176)
d241bc81a [sim] Add DoubleSolenoidSim and SolenoidSim classes (#3177)
cb7f39afa [wpilibc] Add RobotController::GetBatteryVoltage() to C++ (#3179)
99b5ad9eb [wpilibj] Fix warnings that are not unused variables or deprecation (#3161)
c14b23775 [build] Fixup doxygen generated include dirs to match what users would need (#3154)
d447c7dc3 [sim] Add SimDeviceSim ctor overloads (#3134)
247420c9c [build] Remove jcenter repo (#3157)
04b112e00 [build] Include debug info in plugin published artifacts (#3149)
be0ce9900 [examples] Use PWMSparkMax instead of PWMVictorSPX (#3156)
69e8d0b65 [wpilib] Move RomiGyro into main wpilibc/j (#3143)
94e685e1b [wpimath] Add custom residual support to EKF (#3148)
5899f3dd2 [sim] GUI: Make keyboard settings loading more robust (#3167)
f82aa1d56 [wpilib] Fix HolonomicDriveController atReference() behavior (#3163)
fe5c2cf4b [wpimath] Remove ControllerUtil.java (#3169)
43d40c6e9 [wpiutil] Suppress unchecked cast in CombinedRuntimeLoader (#3155)
3d44d8f79 [wpimath] Fix argument order in UKF docs (NFC) (#3147)
ba6fe8ff2 [cscore] Add USB camera change event (#3123)
533725888 [build] Tweak OpenCV cmake search paths to work better on Linux (#3144)
29bf9d6ef [cscore] Add polled support to listener
483beb636 [ntcore] Move CallbackManager to wpiutil
fdaec7759 [examples] Instantiate m_ramseteController in example (#3142)
8494a5761 Rename default branch to main (#3140)
45590eea2 [wpigui] Hardcode window scale to 1 on macOS (#3135)
834a64920 [build] Publish libglass and libglassnt to Maven (#3127)
2c2ccb361 [wpimath] Fix Rotation2d equality operator (#3128)
fb5c8c39a [wpigui] clang-tidy: readability-braces-around-statements
f7d39193a [wpigui] Fix copyright in pfd and wpigui_metal.mm
aec796b21 [ntcore] Fix conditional jump on uninitialized value (#3125)
fb13bb239 [sim] GUI: Add right click popup for keyboard joystick settings (#3119)
c517ec677 [build] Update thirdparty-imgui to 1.79-2 (#3118)
e8cbf2a71 [wpimath] Fix typo in SwerveDrivePoseEstimator doc (NFC) (#3112)
e9c86df46 [wpimath] Add tests for swerve module optimization (#3100)
6ba8c289c [examples] Remove negative of ArcadeDrive(fwd, ..) in the C++ Getting Started Example (#3102)
3f1672e89 [hal] Add SimDevice createInt() and createLong() (#3110)
15be5cbf1 [examples] Fix segfault in GearsBot C++ example (#3111)
4cf0e5e6d Add quick links to API documentation in README (#3082)
6b1898f12 Fix RT priority docs (NFC) (#3098)
b3426e9c0 [wpimath] Fix missing whitespace in pose estimator doc (#3097)
38c1a1f3e [examples] Fix feildRelative -> fieldRelative typo in XControllerCommand examples (#3104)
4488e25f1 [glass] Shorten SmartDashboard window names (#3096)
cfdb3058e [wpilibj] Update SimDeviceSimTest (#3095)
64adff5fe [examples] Fix typo in ArcadeDrive constructor parameter name (#3092)
6efc58e3d [build] Fix issues with build on windows, deprecations, and native utils (#3090)
f393989a5 [wpimath, wpiutil] Add wpi::array for compile time size checking (#3087)
d6ed20c1e [build] Set macOS deployment target to 10.14 (#3088)
7c524014c [hal] Add [[nodiscard]] to HAL_WaitForNotifierAlarm() (#3085)
406d055f0 [wpilib] Fixup wouldHitLowerLimit in elevator and arm simulation classes. (#3076)
04a90b5dd [examples] Don't continually set setpoint in PotentiometerPID Examples (#3084)
8c5bfa013 [sim] GUI: Add max value setting for keyboard joysticks (#3083)
bc80c5535 [hal] Add SimValue reset() function (#3064)
9c3b51ca0 [wpilib] Document simulation APIs (#3079)
26584ff14 [wpimath] Add model description to LinearSystemId Javadocs (#3080)
42c3d5286 [examples] Sync Java and C++ trajectories in sim example (#3081)
64e72f710 [wpilibc] Add missing function RoboRioSim::ResetData (#3073)
e95503798 [wpimath] Add optimize() to SwerveModuleState (#3065)
fb99910c2 [hal] Add SimInt and SimLong wrappers for int/long SimValue (#3066)
e620bd4d3 [doc] Add machine-readable websocket specification (#3059)
a44e761d9 [glass] Add support for plot Y axis labels
ea1974d57 [wpigui] Update imgui and implot to latest
85a0bd43c [wpimath] Add RKF45 integration (#3047)
278e0f126 [glass] Use .controllable to set widgets' read-only state (#3035)
d8652cfd4 [wpimath] Make Java DCMotor API consistent with C++ and fix motor calcs (#3046)
377b7065a [build] Add toggleOffOn to Java spotless (#3053)
1e9c79c58 [sim] Use plant output to retrieve simulated position (#3043)
78147aa34 [sim] GUI: Fix Keyboard Joystick (#3052)
cd4a2265b [ntcore] Fix NetworkTableEntry::GetRaw() (#3051)
767ac1de1 [build] Use deploy key for doc publish (#3048)
d762215d1 [build] Add publish documentation script (#3040)
1fd09593c [examples] Add missing TestInit method to GettingStarted Example (#3039)
e45a0f6ce [examples] Add RomiGyro to the Romi Reference example (#3037)
94f852572 Update imaging link and fix typo (#3038)
d73cf64e5 [examples] Update RomiReference to match motor directions (#3036)
f945462ba Bump copyright year to 2021 (#3033)
b05946175 [wpimath] Catch Drake JNI exceptions and rethrow them (#3032)
62f0f8190 [wpimath] Deduplicate angle modulus functions (#2998)
bf8c0da4b [glass] Add "About" popup with version number (#3031)
dfdd6b389 [build] Increase Gradle heap size in Gazebo build (#3028)
f5e0fc3e9 Finish clang-tidy cleanups (#3003)
d741101fe [sim] Revert accidental commit of WSProvider_PDP.h (#3027)
e1620799c [examples] Add C++ RomiReference example (#2969)
749c7adb1 [command] Fix use-after-free in CommandScheduler (#3024)
921a73391 [sim] Add WS providers for AddressableLED, PCM, and Solenoid (#3026)
26d0004fe [build] Split Actions into different yml files (#3025)
948af6d5b [wpilib] PWMSpeedController.get(): Apply Inversion (#3016)
670a187a3 [wpilibc] SuppliedValueWidget.h: Forward declare ShuffleboardContainer (#3021)
be9f72502 [ntcore] NetworkTableValue: Use std::forward instead of std::move (#3022)
daf3f4cb1 [cscore] cscore_raw_cv.h: Fix error in PutFrame() (#3019)
5acda4cc7 [wpimath] ElevatorFeedforward.h: Add time.h include
8452af606 [wpimath] units/radiation.h: Add mass.h include
630d44952 [hal] ErrorsInternal.h: Add stdint.h include
7372cf7d9 [cscore] Windows NetworkUtil.cpp: Add missing include
b7e46c558 Include .h from .inc/.inl files (NFC) (#3017)
bf8f8710e [examples] Update Romi template and example (#2996)
6ffe5b775 [glass] Ensure NetworkTableTree parent context menu has an id (#3015)
be0805b85 [build] Update to WPILibVersioningPlugin 4.1.0 (#3014)
65b2359b2 [build] Add spotless for other files (#3007)
8651aa73e [examples] Enable NT Flush in Field2d examples (#3013)
78b542737 [build] Add Gazebo build to Actions CI (#3004)
fccf86532 [sim] DriverStationGui: Fix two bugs (#3010)
185741760 [sim] WSProvider_Joystick: Fix off-by-1 in incoming buttons (#3011)
ee7114a58 [glass] Add drive class widgets (#2975)
00fa91d0d [glass] Use ImGui style for gyro widget colors (#3009)
b7a25bfc3 ThirdPartyNotices: Add portable file dialogs license (#3005)
a2e46b9a1 [glass] modernize-use-nullptr (NFC) (#3006)
a751fa22d [build] Apply spotless for java formatting (#1768)
e563a0b7d [wpimath] Make LinearSystemLoop move-constructible and move-assignable (#2967)
49085ca94 [glass] Add context menus to remove and add NetworkTables values (#2979)
560a850a2 [glass] Add NetworkTables Log window (#2997)
66782e231 [sim] Create Left/Right drivetrain current accessors (#3001)
b60eb1544 clang-tidy: bugprone-virtual-near-miss
cbe59fa3b clang-tidy: google-explicit-constructor
c97c6dc06 clang-tidy: google-readability-casting (NFC)
32fa97d68 clang-tidy: modernize-use-nullptr (NFC)
aee460326 clang-tidy: modernize-pass-by-value
29c7da5f1 clang-tidy: modernize-make-unique
6131f4e32 clang-tidy: modernize-concat-nested-namespaces (NFC)
67e03e625 clang-tidy: modernize-use-equals-default
b124f9101 clang-tidy: modernize-use-default-member-init
d11a3a638 clang-tidy: modernize-use-override (NFC)
4cc0706b0 clang-tidy: modernize-use-using (NFC)
885f5a978 [wpilibc] Speed up ScopedTracerTest (#2999)
60b596457 [wpilibj] Fix typos (NFC) (#3000)
6e1919414 [build] Bring naming checkstyle rules up to date with Google Style guide (#1781)
8c8ec5e63 [wpilibj] Suppress unchecked cast warnings (#2995)
b8413ddd5 [wpiutil] Add noexcept to timestamp static functions (#2994)
5d976b6e1 [glass] Load NetworkTableView settings on first draw (#2993)
2b4317452 Replace NOLINT(runtime/explicit) comments with NOLINT (NFC) (#2992)
1c3011ba4 [glass] Fix handling of "/" NetworkTables key (#2991)
574a42f3b [hal] Fix UnsafeManipulateDIO status check (#2987)
9005cd59e [wpilib] Clamp input voltage in sim classes (#2955)
dd494d4ab [glass] NetworkTablesModel::Update(): Avoid use-after-move (#2988)
7cca469a1 [wpimath] NormalizeAngle: Make inline, remove unnamed namespace (#2986)
2aed432b4 Add braces to C++ single-line loops and conditionals (NFC) (#2973)
0291a3ff5 [wpiutil] StringRef: Add noexcept to several constructors (#2984)
5d7315280 [wpimath] Update UnitsTest.cpp copyright (#2985)
254931b9a [wpimath] Remove LinearSystem from LinearSystemLoop (#2968)
aa89744c9 Update OtherVersions.md to include wpimath info (#2983)
1cda3f5ad [glass] Fix styleguide (#2976)
8f1f64ffb Remove year from file copyright message (NFC) (#2972)
2bc0a7795 [examples] Fix wpiformat warning about utility include (#2971)
4204da6ad [glass] Add application icon
7ac39b10f [wpigui] Add icon support
6b567e006 [wpimath] Add support for varying vision standard deviations in pose estimators (#2956)
df299d6ed [wpimath] Add UnscentedKalmanFilter::Correct() overload (#2966)
4e34f0523 [examples] Use ADXRS450_GyroSim class in simulation example (#2964)
9962f6fd7 [wpilib] Give Field2d a default Sendable name (#2953)
f9d492f4b [sim] GUI: Show "Other Devices" window by default (#2961)
a8bb2ef1c [sim] Fix ADXRS450_GyroSim and DutyCycleEncoderSim (#2963)
240c629cd [sim] Try to guess "Map Gamepad" setting (#2960)
952567dd3 [wpilibc] Add missing move constructors and assignment operators (#2959)
10b396b4c [sim] Various WebSockets fixes and enhancements (#2952)
699bbe21a [examples] Fix comments in Gearsbot to match implementation (NFC) (#2957)
27b67deca [glass] Add more widgets (#2947)
581b7ec55 [wpilib] Add option to flush NetworkTables every iterative loop
acfbb1a44 [ntcore] DispatcherBase::Flush: Use wpi::Now()
d85a6d8fe [ntcore] Reduce limit on flush and update rate to 5 ms
20fbb5c63 [sim] Fix stringop truncation warning from GCC 10 (#2945)
1051a06a7 [glass] Show NT timestamps in seconds (#2944)
98dfc2620 [glass] Fix plots (#2943)
1ba0a2ced [sim] GUI: Add keyboard virtual joystick support (#2940)
4afb13f98 [examples] Replace M_PI with wpi::math::pi (#2938)
b27d33675 [examples] Enhance Romi templates (#2931)
00b9ae77f [sim] Change default WS port number to 3300 (#2932)
65219f309 [examples] Update Field2d position in periodic() (#2928)
f78d1d434 [sim] Process WS Encoder reset internally (#2927)
941edca59 [hal] Add Java SimDeviceDataJNI.getSimDeviceName (#2924)
a699435ed [wpilibj] Fix FlywheelSim argument order in constructor (#2922)
66d641718 [examples] Add tasks to run Java examples (#2920)
558e37c41 [examples] Add simple differential drive simulation example (#2918)
4f40d991e [glass] Switch name of Glass back to glass (#2919)
549af9900 [build] Update native-utils to 2021.0.6 (#2914)
b33693009 [glass] Change basename of glass to Glass (#2915)
c9a0edfb8 [glass] Package macOS application bundle
2c5668af4 [wpigui] Add platform-specific preferences save
751dea32a [wpilibc] Try to work around ABI break introduced in #2901 (#2917)
cd8f4bfb1 [build] Package up msvc runtime into maven artifact (#2913)
a6cfcc686 [wpilibc] Move SendableChooser Doxygen comments to header (NFC) (#2911)
b8c4f603d [wpimath] Upgrade to Eigen 3.3.9 (#2910)
0075e4b39 [wpilibj] Fix NPE in Field2d (#2909)
125af556c [simulation] Fix halsim_gui ntcore and wpiutil deps (#2908)
963ad5c25 [wpilib] Add noise to Differential Drive simulator (#2903)
387f56cb7 [examples] Add Romi reference Java example and templates (#2905)
b3deda38c [examples] Zero motors on disabledInit() in sim physics examples (#2906)
2a5ca7745 [glass] Add glass: an application for display of robot data
727940d84 [wpilib] Move Field2d to SmartDashboard
8cd42478e [wpilib] SendableBuilder: Make GetTable() visible
c11d34b26 [command] Use addCommands in command group templates (#2900)
339d7445b [sim] Add HAL hooks for simulationPeriodic (#2881)
d16f05f2c [wpilib] Fix SmartDashboard update order (#2896)
5427b32a4 [wpiutil] unique_function: Restrict implicit conversion (#2899)
f73701239 [ntcore] Add missing SetDefault initializer_list functions (#2898)
f5a6fc070 [sim] Add initialized flag for all solenoids on a PCM (#2897)
bdf5ba91a [wpilibj] Fix typo in ElevatorSim (#2895)
bc8f33877 [wpilib] Add pose estimators (#2867)
3413bfc06 [wpilib] PIDController: Recompute the error in AtSetpoint() (#2822)
2056f0ce0 [wpilib] Fix bugs in Hatchbot examples (#2893)
5eb8cfd69 [wpilibc] Fix MatchDataSender (#2892)
e6a425448 [build] Delete test folders after tests execute (#2891)
d478ad00d [imgui] Allow usage of imgui_stdlib (#2889)
53eda861d [build] Add unit-testing infrastructure to examples (#2863)
cc1d86ba6 [sim] Add title to simulator GUI window (#2888)
f0528f00e [build] CMake: Use project-specific binary and source dirs (#2886)
5cd2ad124 [wpilibc] Add Color::operator!= (#2887)
6c00e7a90 [build] CI CMake: build with GUI enabled (#2884)
53170bbb5 Update roboRIO toolchain installation instructions (#2883)
467258e05 [sim] GUI: Add option to not zero disconnected joysticks (#2876)
129be23c9 Clarify JDK installation instructions in readme (#2882)
8e9290e86 [build] Add separate CMake setting for wpimath (#2885)
7cf5bebf8 [wpilibj] Cache NT writes from DriverStation (#2780)
f7f9087fb [command] Fix timing issue in RamseteCommand (#2871)
256e7904f [wpilibj] SimDeviceSim: Fix sim value changed callback (#2880)
c8ea1b6c3 [wpilib] Add function to adjust LQR controller gain for pure time delay (#2878)
2816b06c0 [sim] HAL_GetControlWord: Fully zero output (#2873)
4c695ea08 Add toolchain installation instructions to README (#2875)
a14d51806 [wpimath] DCMotor: fix doc typo (NFC) (#2868)
017097791 [build] CMake: build sim extensions as shared libs (#2866)
f61726b5a [build] Fix cmake-config files (#2865)
fc27fdac5 [wpilibc] Cache NT values from driver station (#2768)
47c59859e [sim] Make SimDevice callbacks synchronous (#2861)
6e76ab9c0 [build] Turn on WITH_GUI for Windows cmake CI
5f78b7670 [build] Set GLFW_INSTALL to OFF
5e0808c84 [wpigui] Fix Windows cmake build
508f05a47 [imgui] Fix typo in Windows CMake target sources
Change-Id: I1737b45965f31803a96676bedc7dc40e337aa321
git-subtree-dir: third_party/allwpilib
git-subtree-split: e473a00f9785f9949e5ced30901baeaf426d2fc9
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/wpilibjExamples/build.gradle b/wpilibjExamples/build.gradle
index 73185ba..518b1e8 100644
--- a/wpilibjExamples/build.gradle
+++ b/wpilibjExamples/build.gradle
@@ -1,4 +1,7 @@
+import edu.wpi.first.toolchain.NativePlatforms
+
apply plugin: 'java'
+apply plugin: 'jacoco'
ext {
useJava = true
@@ -8,7 +11,19 @@
apply from: "${rootDir}/shared/opencv.gradle"
+test {
+ useJUnitPlatform()
+ systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
+ testLogging {
+ events "failed"
+ exceptionFormat "full"
+ }
+ finalizedBy jacocoTestReport
+}
+if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxraspbian') || project.hasProperty('onlylinuxaarch64bionic')) {
+ test.enabled = false
+}
dependencies {
implementation project(':wpilibj')
@@ -21,9 +36,24 @@
implementation project(':cameraserver')
implementation project(':wpilibOldCommands')
implementation project(':wpilibNewCommands')
+
+ testImplementation 'org.junit.jupiter:junit-jupiter-api:5.4.2'
+ testImplementation 'org.junit.jupiter:junit-jupiter-params:5.4.2'
+ testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.4.2'
}
-if (!project.hasProperty('skipPMD')) {
+jacoco {
+ toolVersion = "0.8.4"
+}
+
+jacocoTestReport {
+ reports {
+ xml.required = true
+ html.required = true
+ }
+}
+
+if (!project.hasProperty('skipJavaFormat')) {
apply plugin: 'pmd'
pmd {
@@ -55,6 +85,116 @@
commandFile = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/commands/commands.json")
}
+apply plugin: 'cpp'
+apply plugin: 'edu.wpi.first.NativeUtils'
+
+apply from: '../shared/config.gradle'
+
+
+model {
+ components {
+ wpilibjExamplesDev(NativeExecutableSpec) {
+ targetBuildTypes 'debug'
+ sources {
+ cpp {
+ source {
+ srcDirs 'src/dev/native/cpp'
+ include '**/*.cpp'
+ }
+ exportedHeaders {
+ srcDirs 'src/dev/native/include'
+ }
+ }
+ }
+ binaries.all { binary ->
+ lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
+ lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+ lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
+ lib project: ':wpimath', library: 'wpimathJNI', linkage: 'shared'
+ lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
+ lib project: ':cscore', library: 'cscore', linkage: 'shared'
+ lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
+ lib project: ':cscore', library: 'cscoreJNIShared', linkage: 'shared'
+ project(':hal').addHalDependency(binary, 'shared')
+ project(':hal').addHalJniDependency(binary)
+ lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+ lib project: ':wpiutil', library: 'wpiutilJNI', 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 {
+ def systemArch = getCurrentArch()
+ if (binary.targetPlatform.name == systemArch) {
+ lib project: ":simulation:halsim_gui", library: 'halsim_gui', linkage: 'shared'
+ }
+ }
+ nativeUtils.useRequiredLibrary(binary, 'opencv_shared')
+ }
+ }
+ }
+ tasks {
+ def c = $.components
+ def found = false
+ c.each {
+ if (it in NativeExecutableSpec && it.name == "wpilibjExamplesDev") {
+ it.binaries.each {
+ if (!found) {
+ def arch = it.targetPlatform.name
+ if (arch == NativePlatforms.desktop) {
+ found = true
+ def filePath = it.tasks.install.installDirectory.get().toString() + File.separatorChar + 'lib'
+
+ def doFirstTask = { task ->
+ def extensions = ''
+ it.tasks.install.installDirectory.get().getAsFile().eachFileRecurse {
+ def name = it.name
+ if (!(name.endsWith('.dll') || name.endsWith('.so') || name.endsWith('.dylib'))) {
+ return
+ }
+ def file = it
+ if (name.startsWith("halsim_gui") || name.startsWith("libhalsim_gui".toString())) {
+ extensions += file.absolutePath + File.pathSeparator
+ }
+ }
+ if (extensions != '') {
+ task.environment 'HALSIM_EXTENSIONS', extensions
+ }
+ }
+
+ project.tasks.create("runCpp", Exec) { task ->
+ dependsOn it.tasks.install
+ commandLine it.tasks.install.runScriptFile.get().asFile.toString()
+ test.dependsOn it.tasks.install
+ test.systemProperty 'java.library.path', filePath
+ test.environment 'LD_LIBRARY_PATH', filePath
+ test.workingDir filePath
+ }
+
+ new groovy.json.JsonSlurper().parseText(exampleFile.text).each { entry ->
+ project.tasks.create("run${entry.foldername}", JavaExec) { run ->
+ mainClass = "edu.wpi.first.wpilibj.examples." + entry.foldername + ".Main"
+ classpath = sourceSets.main.runtimeClasspath
+ run.dependsOn it.tasks.install
+ run.systemProperty 'java.library.path', filePath
+ run.environment 'LD_LIBRARY_PATH', filePath
+ run.workingDir filePath
+ doFirst { doFirstTask(run) }
+
+ if (org.gradle.internal.os.OperatingSystem.current().isMacOsX()) {
+ run.jvmArgs = ['-XstartOnFirstThread']
+ }
+ }
+ }
+
+ found = true
+ }
+ }
+ }
+ }
+ }
+ }
+}
+
ext {
isCppCommands = false
}
diff --git a/wpilibjExamples/src/dev/native/cpp/main.cpp b/wpilibjExamples/src/dev/native/cpp/main.cpp
new file mode 100644
index 0000000..a3e363e
--- /dev/null
+++ b/wpilibjExamples/src/dev/native/cpp/main.cpp
@@ -0,0 +1,5 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+int main() {}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command/ReplaceMeCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command/ReplaceMeCommand.java
index c17ea2d..5ca1ca1 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command/ReplaceMeCommand.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command/ReplaceMeCommand.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.command;
@@ -17,13 +14,11 @@
// Called just before this Command runs the first time
@Override
- protected void initialize() {
- }
+ protected void initialize() {}
// Called repeatedly when this Command is scheduled to run
@Override
- protected void execute() {
- }
+ protected void execute() {}
// Make this return true when this Command no longer needs to run execute()
@Override
@@ -33,12 +28,10 @@
// Called once after isFinished returns true
@Override
- protected void end() {
- }
+ protected void end() {}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
- protected void interrupted() {
- }
+ protected void interrupted() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command2/ReplaceMeCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command2/ReplaceMeCommand.java
index 5d3a501..a9a96ef 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command2/ReplaceMeCommand.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command2/ReplaceMeCommand.java
@@ -1,36 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.command2;
import edu.wpi.first.wpilibj2.command.CommandBase;
public class ReplaceMeCommand extends CommandBase {
- /**
- * Creates a new ReplaceMeCommand.
- */
+ /** Creates a new ReplaceMeCommand. */
public ReplaceMeCommand() {
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
- public void initialize() {
- }
+ public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
- public void execute() {
- }
+ public void execute() {}
// Called once the command ends or is interrupted.
@Override
- public void end(boolean interrupted) {
- }
+ public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commandgroup/ReplaceMeCommandGroup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commandgroup/ReplaceMeCommandGroup.java
index d1dae8c..ba8e273 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commandgroup/ReplaceMeCommandGroup.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commandgroup/ReplaceMeCommandGroup.java
@@ -1,18 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.commandgroup;
import edu.wpi.first.wpilibj.command.CommandGroup;
public class ReplaceMeCommandGroup extends CommandGroup {
- /**
- * Add your docs here.
- */
+ /** Add your docs here. */
public ReplaceMeCommandGroup() {
// Add Commands here:
// e.g. addSequential(new Command1());
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json
index a26d15b..8165a3c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json
@@ -202,7 +202,7 @@
},
{
"name": "TrapezoidProfileSubsystem (New)",
- "description": "A command that executes a trapezoidal motion profile.",
+ "description": "A subystem that executes a trapezoidal motion profile.",
"tags": [
"trapezoidprofilesubsystem"
],
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/emptyclass/ReplaceMeEmptyClass.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/emptyclass/ReplaceMeEmptyClass.java
index e2d866e..2e8a5ff 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/emptyclass/ReplaceMeEmptyClass.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/emptyclass/ReplaceMeEmptyClass.java
@@ -1,14 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.emptyclass;
-/**
- * Add your docs here.
- */
-public class ReplaceMeEmptyClass {
-}
+/** Add your docs here. */
+public class ReplaceMeEmptyClass {}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instant/ReplaceMeInstantCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instant/ReplaceMeInstantCommand.java
index bf24a14..769b595 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instant/ReplaceMeInstantCommand.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instant/ReplaceMeInstantCommand.java
@@ -1,21 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.instant;
import edu.wpi.first.wpilibj.command.InstantCommand;
-/**
- * Add your docs here.
- */
+/** Add your docs here. */
public class ReplaceMeInstantCommand extends InstantCommand {
- /**
- * Add your docs here.
- */
+ /** Add your docs here. */
public ReplaceMeInstantCommand() {
super();
// Use requires() here to declare subsystem dependencies
@@ -24,7 +17,5 @@
// Called once when the command executes
@Override
- protected void initialize() {
- }
-
+ protected void initialize() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instantcommand/ReplaceMeInstantCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instantcommand/ReplaceMeInstantCommand.java
index 4a8b0fc..fe7fe9b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instantcommand/ReplaceMeInstantCommand.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instantcommand/ReplaceMeInstantCommand.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.instantcommand;
@@ -19,6 +16,5 @@
// Called when the command is initially scheduled.
@Override
- public void initialize() {
- }
+ public void initialize() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.java
index 92958b3..ab4d0cc 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.parallelcommandgroup;
@@ -13,11 +10,10 @@
// information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class ReplaceMeParallelCommandGroup extends ParallelCommandGroup {
- /**
- * Creates a new ReplaceMeParallelCommandGroup.
- */
+ /** Creates a new ReplaceMeParallelCommandGroup. */
public ReplaceMeParallelCommandGroup() {
- // Add your commands in the super() call, e.g.
- // super(new FooCommand(), new BarCommand());super();
+ // Add your commands in the addCommands() call, e.g.
+ // addCommands(new FooCommand(), new BarCommand());
+ addCommands();
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.java
index 82c000e..95c902d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.paralleldeadlinegroup;
@@ -14,13 +11,11 @@
// information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class ReplaceMeParallelDeadlineGroup extends ParallelDeadlineGroup {
- /**
- * Creates a new ReplaceMeParallelDeadlineGroup.
- */
+ /** Creates a new ReplaceMeParallelDeadlineGroup. */
public ReplaceMeParallelDeadlineGroup() {
- // Add your commands in the super() call. Add the deadline first.
- super(
- new InstantCommand()
- );
+ // Add the deadline command in the super() call. Add other commands using
+ // addCommands().
+ super(new InstantCommand());
+ // addCommands(new FooCommand(), new BarCommand());
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java
index f710ee4..3de0c7f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.parallelracegroup;
@@ -13,12 +10,10 @@
// information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class ReplaceMeParallelRaceGroup extends ParallelRaceGroup {
- /**
- * Creates a new ReplaceMeParallelRaceGroup.
- */
+ /** Creates a new ReplaceMeParallelRaceGroup. */
public ReplaceMeParallelRaceGroup() {
- // Add your commands in the super() call, e.g.
- // super(new FooCommand(), new BarCommand());
- super();
+ // Add your commands in the addCommands() call, e.g.
+ // addCommands(new FooCommand(), new BarCommand());
+ addCommands();
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidcommand/ReplaceMePIDCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidcommand/ReplaceMePIDCommand.java
index db1584b..ff39b12 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidcommand/ReplaceMePIDCommand.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidcommand/ReplaceMePIDCommand.java
@@ -1,22 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.pidcommand;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj2.command.PIDCommand;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class ReplaceMePIDCommand extends PIDCommand {
- /**
- * Creates a new ReplaceMePIDCommand.
- */
+ /** Creates a new ReplaceMePIDCommand. */
public ReplaceMePIDCommand() {
super(
// The controller that the command will use
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem/ReplaceMePIDSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem/ReplaceMePIDSubsystem.java
index 46d75f6..01e7987 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem/ReplaceMePIDSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem/ReplaceMePIDSubsystem.java
@@ -1,21 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.pidsubsystem;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
-/**
- * Add your docs here.
- */
+/** Add your docs here. */
public class ReplaceMePIDSubsystem extends PIDSubsystem {
- /**
- * Add your docs here.
- */
+ /** Add your docs here. */
public ReplaceMePIDSubsystem() {
// Intert a subsystem name and PID values here
super("SubsystemName", 1, 2, 3);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem2/ReplaceMePIDSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem2/ReplaceMePIDSubsystem.java
index 7d5e7b3..0876d94 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem2/ReplaceMePIDSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem2/ReplaceMePIDSubsystem.java
@@ -1,19 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.pidsubsystem2;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;
public class ReplaceMePIDSubsystem extends PIDSubsystem {
- /**
- * Creates a new ReplaceMePIDSubsystem.
- */
+ /** Creates a new ReplaceMePIDSubsystem. */
public ReplaceMePIDSubsystem() {
super(
// The PIDController used by the subsystem
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.java
index 1fc4c2d..f3d4410 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.java
@@ -1,29 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.profiledpidcommand;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class ReplaceMeProfiledPIDCommand extends ProfiledPIDCommand {
- /**
- * Creates a new ReplaceMeProfiledPIDCommand.
- */
+ /** Creates a new ReplaceMeProfiledPIDCommand. */
public ReplaceMeProfiledPIDCommand() {
super(
// The ProfiledPIDController used by the command
new ProfiledPIDController(
// The PID gains
- 0, 0, 0,
+ 0,
+ 0,
+ 0,
// The motion profile constraints
new TrapezoidProfile.Constraints(0, 0)),
// This should return the measurement
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.java
index b380d4e..60eda35 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.java
@@ -1,26 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.profiledpidsubsystem;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
public class ReplaceMeProfiledPIDSubsystem extends ProfiledPIDSubsystem {
- /**
- * Creates a new ReplaceMeProfiledPIDSubsystem.
- */
+ /** Creates a new ReplaceMeProfiledPIDSubsystem. */
public ReplaceMeProfiledPIDSubsystem() {
super(
// The ProfiledPIDController used by the subsystem
- new ProfiledPIDController(0, 0, 0,
- // The motion profile constraints
- new TrapezoidProfile.Constraints(0, 0)));
+ new ProfiledPIDController(
+ 0,
+ 0,
+ 0,
+ // The motion profile constraints
+ new TrapezoidProfile.Constraints(0, 0)));
}
@Override
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java
index 1038b04..ca4488b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.sequentialcommandgroup;
@@ -13,12 +10,10 @@
// information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class ReplaceMeSequentialCommandGroup extends SequentialCommandGroup {
- /**
- * Creates a new ReplaceMeSequentialCommandGroup.
- */
+ /** Creates a new ReplaceMeSequentialCommandGroup. */
public ReplaceMeSequentialCommandGroup() {
- // Add your commands in the super() call, e.g.
- // super(new FooCommand(), new BarCommand());
- super();
+ // Add your commands in the addCommands() call, e.g.
+ // addCommands(new FooCommand(), new BarCommand());
+ addCommands();
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem/ReplaceMeSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem/ReplaceMeSubsystem.java
index 1c7fe6d..671daf1 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem/ReplaceMeSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem/ReplaceMeSubsystem.java
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.subsystem;
import edu.wpi.first.wpilibj.command.Subsystem;
-/**
- * Add your docs here.
- */
+/** Add your docs here. */
public class ReplaceMeSubsystem extends Subsystem {
// Put methods for controlling this subsystem
// here. Call these from Commands.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem2/ReplaceMeSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem2/ReplaceMeSubsystem.java
index 0fa5838..f72539d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem2/ReplaceMeSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem2/ReplaceMeSubsystem.java
@@ -1,21 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.subsystem2;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class ReplaceMeSubsystem extends SubsystemBase {
- /**
- * Creates a new ReplaceMeSubsystem.
- */
- public ReplaceMeSubsystem() {
-
- }
+ /** Creates a new ReplaceMeSubsystem. */
+ public ReplaceMeSubsystem() {}
@Override
public void periodic() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/timed/ReplaceMeTimedCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/timed/ReplaceMeTimedCommand.java
index 41ba965..6021382 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/timed/ReplaceMeTimedCommand.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/timed/ReplaceMeTimedCommand.java
@@ -1,21 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.timed;
import edu.wpi.first.wpilibj.command.TimedCommand;
-/**
- * Add your docs here.
- */
+/** Add your docs here. */
public class ReplaceMeTimedCommand extends TimedCommand {
- /**
- * Add your docs here.
- */
+ /** Add your docs here. */
public ReplaceMeTimedCommand(double timeout) {
super(timeout);
// Use requires() here to declare subsystem dependencies
@@ -24,22 +17,18 @@
// Called just before this Command runs the first time
@Override
- protected void initialize() {
- }
+ protected void initialize() {}
// Called repeatedly when this Command is scheduled to run
@Override
- protected void execute() {
- }
+ protected void execute() {}
// Called once after timeout
@Override
- protected void end() {
- }
+ protected void end() {}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
- protected void interrupted() {
- }
+ protected void interrupted() {}
}
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 646817f..7a55719 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
@@ -1,22 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.trapezoidprofilecommand;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class ReplaceMeTrapezoidProfileCommand extends TrapezoidProfileCommand {
- /**
- * Creates a new ReplaceMeTrapezoidProfileCommand.
- */
+ /** Creates a new ReplaceMeTrapezoidProfileCommand. */
public ReplaceMeTrapezoidProfileCommand() {
super(
// The motion profile to be executed
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.java
index 46f1b44..e95f957 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.java
@@ -1,19 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.trapezoidprofilesubsystem;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;
public class ReplaceMeTrapezoidProfileSubsystem extends TrapezoidProfileSubsystem {
- /**
- * Creates a new ReplaceMeTrapezoidProfileSubsystem.
- */
+ /** Creates a new ReplaceMeTrapezoidProfileSubsystem. */
public ReplaceMeTrapezoidProfileSubsystem() {
super(
// The constraints for the generated profiles
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trigger/ReplaceMeTrigger.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trigger/ReplaceMeTrigger.java
index 713ac6d..c1343d9 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trigger/ReplaceMeTrigger.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trigger/ReplaceMeTrigger.java
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.commands.trigger;
import edu.wpi.first.wpilibj.buttons.Trigger;
-/**
- * Add your docs here.
- */
+/** Add your docs here. */
public class ReplaceMeTrigger extends Trigger {
@Override
public boolean get() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Main.java
index 2feaca2..be4ecff 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Main.java
@@ -1,29 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.addressableled;
-
-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);
- }
-}
+// 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.addressableled;
+
+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/addressableled/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java
index a26031b..5995cd0 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java
@@ -1,59 +1,56 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.addressableled;
-
-import edu.wpi.first.wpilibj.AddressableLED;
-import edu.wpi.first.wpilibj.AddressableLEDBuffer;
-import edu.wpi.first.wpilibj.TimedRobot;
-
-public class Robot extends TimedRobot {
- private AddressableLED m_led;
- private AddressableLEDBuffer m_ledBuffer;
- // Store what the last hue of the first pixel is
- private int m_rainbowFirstPixelHue;
-
- @Override
- public void robotInit() {
- // PWM port 9
- // Must be a PWM header, not MXP or DIO
- m_led = new AddressableLED(9);
-
- // Reuse buffer
- // Default to a length of 60, start empty output
- // Length is expensive to set, so only set it once, then just update data
- m_ledBuffer = new AddressableLEDBuffer(60);
- m_led.setLength(m_ledBuffer.getLength());
-
- // Set the data
- m_led.setData(m_ledBuffer);
- m_led.start();
- }
-
- @Override
- public void robotPeriodic() {
- // Fill the buffer with a rainbow
- rainbow();
- // Set the LEDs
- m_led.setData(m_ledBuffer);
- }
-
- private void rainbow() {
- // For every pixel
- for (var i = 0; i < m_ledBuffer.getLength(); i++) {
- // Calculate the hue - hue is easier for rainbows because the color
- // shape is a circle so only one value needs to precess
- final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_ledBuffer.getLength())) % 180;
- // Set the value
- m_ledBuffer.setHSV(i, hue, 255, 128);
- }
- // Increase by to make the rainbow "move"
- m_rainbowFirstPixelHue += 3;
- // Check bounds
- m_rainbowFirstPixelHue %= 180;
- }
-}
+// 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.addressableled;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.TimedRobot;
+
+public class Robot extends TimedRobot {
+ private AddressableLED m_led;
+ private AddressableLEDBuffer m_ledBuffer;
+ // Store what the last hue of the first pixel is
+ private int m_rainbowFirstPixelHue;
+
+ @Override
+ public void robotInit() {
+ // PWM port 9
+ // Must be a PWM header, not MXP or DIO
+ m_led = new AddressableLED(9);
+
+ // Reuse buffer
+ // Default to a length of 60, start empty output
+ // Length is expensive to set, so only set it once, then just update data
+ m_ledBuffer = new AddressableLEDBuffer(60);
+ m_led.setLength(m_ledBuffer.getLength());
+
+ // Set the data
+ m_led.setData(m_ledBuffer);
+ m_led.start();
+ }
+
+ @Override
+ public void robotPeriodic() {
+ // Fill the buffer with a rainbow
+ rainbow();
+ // Set the LEDs
+ m_led.setData(m_ledBuffer);
+ }
+
+ private void rainbow() {
+ // For every pixel
+ for (var i = 0; i < m_ledBuffer.getLength(); i++) {
+ // Calculate the hue - hue is easier for rainbows because the color
+ // shape is a circle so only one value needs to precess
+ final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_ledBuffer.getLength())) % 180;
+ // Set the value
+ m_ledBuffer.setHSV(i, hue, 255, 128);
+ }
+ // Increase by to make the rainbow "move"
+ m_rainbowFirstPixelHue += 3;
+ // Check bounds
+ m_rainbowFirstPixelHue %= 180;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Main.java
index 438697e..b451bc3 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.arcadedrive;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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 f002618..e35243d 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
@@ -1,24 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.arcadedrive;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
- * This is a demo program showing the use of the DifferentialDrive class.
- * Runs the motors with arcade steering.
+ * This is a demo program showing the use of the DifferentialDrive class. Runs the motors with
+ * arcade steering.
*/
public class Robot extends TimedRobot {
- private final PWMVictorSPX m_leftMotor = new PWMVictorSPX(0);
- private final PWMVictorSPX m_rightMotor = new PWMVictorSPX(1);
+ private final 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 Joystick m_stick = new Joystick(0);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Main.java
index fb5c2ab..2088c7b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.arcadedrivexboxcontroller;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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 915d77e..0acc8dc 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
@@ -1,25 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.arcadedrivexboxcontroller;
-import edu.wpi.first.wpilibj.GenericHID.Hand;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
- * This is a demo program showing the use of the DifferentialDrive class.
- * Runs the motors with split arcade steering and an Xbox controller.
+ * This is a demo program showing the use of the DifferentialDrive class. Runs the motors with split
+ * arcade steering and an Xbox controller.
*/
public class Robot extends TimedRobot {
- private final PWMVictorSPX m_leftMotor = new PWMVictorSPX(0);
- private final PWMVictorSPX m_rightMotor = new PWMVictorSPX(1);
+ 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 XboxController m_driverController = new XboxController(0);
@@ -28,7 +24,6 @@
// Drive with split arcade drive.
// That means that the Y axis of the left stick moves forward
// and backward, and the X of the right stick turns left and right.
- m_robotDrive.arcadeDrive(m_driverController.getY(Hand.kLeft),
- m_driverController.getX(Hand.kRight));
+ m_robotDrive.arcadeDrive(m_driverController.getLeftY(), m_driverController.getRightX());
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java
index fda1290..c7f23de 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.armbot;
/**
* 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.
+ * 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.
@@ -22,8 +19,8 @@
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
- public static final int[] kLeftEncoderPorts = new int[]{0, 1};
- public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+ public static final int[] kRightEncoderPorts = new int[] {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
@@ -48,7 +45,7 @@
public static final double kMaxVelocityRadPerSecond = 3;
public static final double kMaxAccelerationRadPerSecSquared = 10;
- public static final int[] kEncoderPorts = new int[]{4, 5};
+ public static final int[] kEncoderPorts = new int[] {4, 5};
public static final int kEncoderPPR = 256;
public static final double kEncoderDistancePerPulse = 2.0 * Math.PI / kEncoderPPR;
@@ -63,6 +60,6 @@
}
public static final class OIConstants {
- public static final int kDriverControllerPort = 1;
+ public static final int kDriverControllerPort = 0;
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java
index 681a38e..4d353a6 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.armbot;
@@ -15,8 +12,7 @@
* call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java
index 8c9cef6..3f91253 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.armbot;
@@ -37,8 +34,8 @@
* 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.
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
@@ -49,21 +46,16 @@
CommandScheduler.getInstance().run();
}
- /**
- * This function is called once each time the robot enters Disabled mode.
- */
+ /** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {
m_robotContainer.disablePIDSubsystems();
}
@Override
- public void disabledPeriodic() {
- }
+ public void disabledPeriodic() {}
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -81,12 +73,9 @@
}
}
- /**
- * This function is called periodically during autonomous.
- */
+ /** This function is called periodically during autonomous. */
@Override
- public void autonomousPeriodic() {
- }
+ public void autonomousPeriodic() {}
@Override
public void teleopInit() {
@@ -99,13 +88,9 @@
}
}
- /**
- * This function is called periodically during operator control.
- */
+ /** This function is called periodically during operator control. */
@Override
- public void teleopPeriodic() {
-
- }
+ public void teleopPeriodic() {}
@Override
public void testInit() {
@@ -113,10 +98,7 @@
CommandScheduler.getInstance().cancelAll();
}
- /**
- * This function is called periodically during test mode.
- */
+ /** This function is called periodically during test mode. */
@Override
- public void testPeriodic() {
- }
+ public void testPeriodic() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
index afe7317..b0d019e 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
@@ -1,30 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.armbot;
-import edu.wpi.first.wpilibj.GenericHID;
+import static edu.wpi.first.wpilibj.XboxController.Button;
+
import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.examples.armbot.Constants.OIConstants;
+import edu.wpi.first.wpilibj.examples.armbot.subsystems.ArmSubsystem;
+import edu.wpi.first.wpilibj.examples.armbot.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import edu.wpi.first.wpilibj.examples.armbot.Constants.OIConstants;
-import edu.wpi.first.wpilibj.examples.armbot.subsystems.ArmSubsystem;
-import edu.wpi.first.wpilibj.examples.armbot.subsystems.DriveSubsystem;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
-
/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * 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.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems
@@ -34,9 +29,7 @@
// The driver's controller
XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
- /**
- * The container for the robot. Contains subsystems, OI devices, and commands.
- */
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
@@ -46,47 +39,50 @@
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
- new RunCommand(() -> m_robotDrive
- .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
- m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
-
+ new RunCommand(
+ () ->
+ m_robotDrive.arcadeDrive(
+ m_driverController.getLeftY(), m_driverController.getRightX()),
+ m_robotDrive));
}
/**
- * Use this method to define your button->command mappings. Buttons can be created by
- * instantiating a {@link GenericHID} or one of its subclasses ({@link
- * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
- * {@link JoystickButton}.
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+ * JoystickButton}.
*/
private void configureButtonBindings() {
-
// Move the arm to 2 radians above horizontal when the 'A' button is pressed.
new JoystickButton(m_driverController, Button.kA.value)
- .whenPressed(() -> {
- m_robotArm.setGoal(2);
- m_robotArm.enable();
- }, m_robotArm);
+ .whenPressed(
+ () -> {
+ m_robotArm.setGoal(2);
+ m_robotArm.enable();
+ },
+ m_robotArm);
// Move the arm to neutral position when the 'B' button is pressed.
new JoystickButton(m_driverController, Button.kB.value)
- .whenPressed(() -> {
- m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads);
- m_robotArm.enable();
- }, m_robotArm);
+ .whenPressed(
+ () -> {
+ m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads);
+ m_robotArm.enable();
+ },
+ m_robotArm);
// Disable the arm controller when Y is pressed.
- new JoystickButton(m_driverController, Button.kY.value)
- .whenPressed(m_robotArm::disable);
+ new JoystickButton(m_driverController, Button.kY.value).whenPressed(m_robotArm::disable);
// Drive at half speed when the bumper is held
- new JoystickButton(m_driverController, Button.kBumperRight.value)
+ new JoystickButton(m_driverController, Button.kRightBumper.value)
.whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
}
/**
- * Disables all ProfiledPIDSubsystem and PIDSubsystem instances. This
- * should be called on robot disable to prevent integral windup.
+ * Disables all ProfiledPIDSubsystem and PIDSubsystem instances. This should be called on robot
+ * disable to prevent integral windup.
*/
public void disablePIDSubsystems() {
m_robotArm.disable();
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
index d9fd206..298b2d5 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
@@ -1,38 +1,38 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.armbot.subsystems;
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.controller.ArmFeedforward;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
-import edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants;
-
-/**
- * A robot arm subsystem that moves with a motion profile.
- */
+/** A robot arm subsystem that moves with a motion profile. */
public class ArmSubsystem extends ProfiledPIDSubsystem {
- private final PWMVictorSPX m_motor = new PWMVictorSPX(ArmConstants.kMotorPort);
+ private final PWMSparkMax m_motor = new PWMSparkMax(ArmConstants.kMotorPort);
private final Encoder m_encoder =
new Encoder(ArmConstants.kEncoderPorts[0], ArmConstants.kEncoderPorts[1]);
private final ArmFeedforward m_feedforward =
- new ArmFeedforward(ArmConstants.kSVolts, ArmConstants.kCosVolts,
- ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
+ new ArmFeedforward(
+ ArmConstants.kSVolts, ArmConstants.kCosVolts,
+ ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
- /**
- * Create a new ArmSubsystem.
- */
+ /** Create a new ArmSubsystem. */
public ArmSubsystem() {
- super(new ProfiledPIDController(ArmConstants.kP, 0, 0, new TrapezoidProfile.Constraints(
- ArmConstants.kMaxVelocityRadPerSecond, ArmConstants.kMaxAccelerationRadPerSecSquared)), 0);
+ super(
+ new ProfiledPIDController(
+ ArmConstants.kP,
+ 0,
+ 0,
+ new TrapezoidProfile.Constraints(
+ ArmConstants.kMaxVelocityRadPerSecond,
+ ArmConstants.kMaxAccelerationRadPerSecSquared)),
+ 0);
m_encoder.setDistancePerPulse(ArmConstants.kEncoderDistancePerPulse);
// Start arm at rest in neutral position
setGoal(ArmConstants.kArmOffsetRads);
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 9404013..9338f30 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
@@ -1,47 +1,47 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.armbot.subsystems;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
import 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 SpeedControllerGroup m_leftMotors =
- new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
- new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
+ private final MotorControllerGroup m_leftMotors =
+ new MotorControllerGroup(
+ new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+ new PWMSparkMax(DriveConstants.kLeftMotor2Port));
// The motors on the right side of the drive.
- private final SpeedControllerGroup m_rightMotors =
- new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
- new PWMVictorSPX(DriveConstants.kRightMotor2Port));
+ private final MotorControllerGroup m_rightMotors =
+ new MotorControllerGroup(
+ new PWMSparkMax(DriveConstants.kRightMotor1Port),
+ new PWMSparkMax(DriveConstants.kRightMotor2Port));
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
// The left-side drive encoder
private final Encoder m_leftEncoder =
- new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
- DriveConstants.kLeftEncoderReversed);
+ 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);
+ new Encoder(
+ DriveConstants.kRightEncoderPorts[0],
+ DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
- /**
- * Creates a new DriveSubsystem.
- */
+ /** Creates a new DriveSubsystem. */
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -58,9 +58,7 @@
m_drive.arcadeDrive(fwd, rot);
}
- /**
- * Resets the drive encoders to currently read a position of 0.
- */
+ /** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
@@ -94,7 +92,7 @@
}
/**
- * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java
index baa15b6..4fd17f1 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.armbotoffboard;
/**
* 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.
+ * 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.
@@ -22,8 +19,8 @@
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
- public static final int[] kLeftEncoderPorts = new int[]{0, 1};
- public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+ public static final int[] kRightEncoderPorts = new int[] {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
@@ -54,6 +51,6 @@
}
public static final class OIConstants {
- public static final int kDriverControllerPort = 1;
+ public static final int kDriverControllerPort = 0;
}
}
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 153a254..4f95b40 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
@@ -1,20 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.armbotoffboard;
-import edu.wpi.first.wpilibj.SpeedController;
+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 SpeedController {
+public class ExampleSmartMotorController implements MotorController {
public enum PIDMode {
kPosition,
kVelocity,
@@ -27,8 +24,7 @@
* @param port The port for the controller.
*/
@SuppressWarnings("PMD.UnusedFormalParameter")
- public ExampleSmartMotorController(int port) {
- }
+ public ExampleSmartMotorController(int port) {}
/**
* Example method for setting the PID gains of the smart controller.
@@ -37,8 +33,7 @@
* @param ki The integral gain.
* @param kd The derivative gain.
*/
- public void setPID(double kp, double ki, double kd) {
- }
+ public void setPID(double kp, double ki, double kd) {}
/**
* Example method for setting the setpoint of the smart controller in PID mode.
@@ -47,16 +42,14 @@
* @param setpoint The controller setpoint.
* @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
*/
- public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {
- }
+ public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
/**
* Places this motor controller in follower mode.
*
* @param leader The leader to follow.
*/
- public void follow(ExampleSmartMotorController leader) {
- }
+ public void follow(ExampleSmartMotorController leader) {}
/**
* Returns the encoder distance.
@@ -76,15 +69,11 @@
return 0;
}
- /**
- * Resets the encoder to zero distance.
- */
- public void resetEncoder() {
- }
+ /** Resets the encoder to zero distance. */
+ public void resetEncoder() {}
@Override
- public void set(double speed) {
- }
+ public void set(double speed) {}
@Override
public double get() {
@@ -92,9 +81,7 @@
}
@Override
- public void setInverted(boolean isInverted) {
-
- }
+ public void setInverted(boolean isInverted) {}
@Override
public boolean getInverted() {
@@ -102,14 +89,8 @@
}
@Override
- public void disable() {
- }
+ public void disable() {}
@Override
- public void stopMotor() {
- }
-
- @Override
- public void pidWrite(double output) {
- }
+ public void stopMotor() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java
index ff9e928..ad36fce 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.armbotoffboard;
@@ -15,8 +12,7 @@
* call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java
index 8a7f99b..e0c0c7b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.armbotoffboard;
@@ -37,8 +34,8 @@
* 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.
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
@@ -49,20 +46,14 @@
CommandScheduler.getInstance().run();
}
- /**
- * This function is called once each time the robot enters Disabled mode.
- */
+ /** This function is called once each time the robot enters Disabled mode. */
@Override
- public void disabledInit() {
- }
+ public void disabledInit() {}
@Override
- public void disabledPeriodic() {
- }
+ public void disabledPeriodic() {}
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
}
}
- /**
- * This function is called periodically during autonomous.
- */
+ /** This function is called periodically during autonomous. */
@Override
- public void autonomousPeriodic() {
- }
+ public void autonomousPeriodic() {}
@Override
public void teleopInit() {
@@ -98,13 +86,9 @@
}
}
- /**
- * This function is called periodically during operator control.
- */
+ /** This function is called periodically during operator control. */
@Override
- public void teleopPeriodic() {
-
- }
+ public void teleopPeriodic() {}
@Override
public void testInit() {
@@ -112,10 +96,7 @@
CommandScheduler.getInstance().cancelAll();
}
- /**
- * This function is called periodically during test mode.
- */
+ /** This function is called periodically during test mode. */
@Override
- public void testPeriodic() {
- }
+ public void testPeriodic() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java
index 6bca84a..b0c3f76 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java
@@ -1,30 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.armbotoffboard;
-import edu.wpi.first.wpilibj.GenericHID;
+import static edu.wpi.first.wpilibj.XboxController.Button;
+
import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.OIConstants;
+import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.ArmSubsystem;
+import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.OIConstants;
-import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.ArmSubsystem;
-import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.DriveSubsystem;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
-
/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * 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.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems
@@ -34,9 +29,7 @@
// The driver's controller
XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
- /**
- * The container for the robot. Contains subsystems, OI devices, and commands.
- */
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
@@ -46,20 +39,20 @@
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
- new RunCommand(() -> m_robotDrive
- .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
- m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
-
+ new RunCommand(
+ () ->
+ m_robotDrive.arcadeDrive(
+ m_driverController.getLeftY(), m_driverController.getRightX()),
+ m_robotDrive));
}
/**
- * Use this method to define your button->command mappings. Buttons can be created by
- * instantiating a {@link GenericHID} or one of its subclasses ({@link
- * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
- * {@link JoystickButton}.
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+ * JoystickButton}.
*/
private void configureButtonBindings() {
-
// Move the arm to 2 radians above horizontal when the 'A' button is pressed.
new JoystickButton(m_driverController, Button.kA.value)
.whenPressed(() -> m_robotArm.setGoal(2), m_robotArm);
@@ -69,12 +62,11 @@
.whenPressed(() -> m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads), m_robotArm);
// Drive at half speed when the bumper is held
- new JoystickButton(m_driverController, Button.kBumperRight.value)
+ new JoystickButton(m_driverController, Button.kRightBumper.value)
.whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
}
-
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
index 92fd43c..152700e 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
@@ -1,36 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.armbotoffboard.subsystems;
-import edu.wpi.first.wpilibj.controller.ArmFeedforward;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;
-
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants;
import edu.wpi.first.wpilibj.examples.armbotoffboard.ExampleSmartMotorController;
+import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;
-/**
- * A robot arm subsystem that moves with a motion profile.
- */
+/** A robot arm subsystem that moves with a motion profile. */
public class ArmSubsystem extends TrapezoidProfileSubsystem {
private final ExampleSmartMotorController m_motor =
new ExampleSmartMotorController(ArmConstants.kMotorPort);
private final ArmFeedforward m_feedforward =
- new ArmFeedforward(ArmConstants.kSVolts, ArmConstants.kCosVolts,
- ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
+ new ArmFeedforward(
+ ArmConstants.kSVolts, ArmConstants.kCosVolts,
+ ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
- /**
- * Create a new ArmSubsystem.
- */
+ /** Create a new ArmSubsystem. */
public ArmSubsystem() {
- super(new TrapezoidProfile.Constraints(ArmConstants.kMaxVelocityRadPerSecond,
- ArmConstants.kMaxAccelerationRadPerSecSquared),
- ArmConstants.kArmOffsetRads);
+ super(
+ new TrapezoidProfile.Constraints(
+ ArmConstants.kMaxVelocityRadPerSecond, ArmConstants.kMaxAccelerationRadPerSecSquared),
+ ArmConstants.kArmOffsetRads);
m_motor.setPID(ArmConstants.kP, 0, 0);
}
@@ -39,7 +33,7 @@
// Calculate the feedforward from the sepoint
double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
// Add the feedforward to the PID output to get the motor output
- m_motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, setpoint.position,
- feedforward / 12.0);
+ m_motor.setSetpoint(
+ ExampleSmartMotorController.PIDMode.kPosition, setpoint.position, feedforward / 12.0);
}
}
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 433dba4..8017719 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
@@ -1,47 +1,47 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.armbotoffboard.subsystems;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
import 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.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
- private final SpeedControllerGroup m_leftMotors =
- new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
- new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
+ private final MotorControllerGroup m_leftMotors =
+ new MotorControllerGroup(
+ new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+ new PWMSparkMax(DriveConstants.kLeftMotor2Port));
// The motors on the right side of the drive.
- private final SpeedControllerGroup m_rightMotors =
- new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
- new PWMVictorSPX(DriveConstants.kRightMotor2Port));
+ private final MotorControllerGroup m_rightMotors =
+ new MotorControllerGroup(
+ new PWMSparkMax(DriveConstants.kRightMotor1Port),
+ new PWMSparkMax(DriveConstants.kRightMotor2Port));
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
// The left-side drive encoder
private final Encoder m_leftEncoder =
- new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
- DriveConstants.kLeftEncoderReversed);
+ 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);
+ new Encoder(
+ DriveConstants.kRightEncoderPorts[0],
+ DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
- /**
- * Creates a new DriveSubsystem.
- */
+ /** Creates a new DriveSubsystem. */
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -58,9 +58,7 @@
m_drive.arcadeDrive(fwd, rot);
}
- /**
- * Resets the drive encoders to currently read a position of 0.
- */
+ /** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
@@ -94,7 +92,7 @@
}
/**
- * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java
index 31777a6..b980695 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.armsimulation;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java
index c906381..a7e4d7d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java
@@ -1,29 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.armsimulation;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.simulation.RoboRioSim;
-import edu.wpi.first.wpilibj.simulation.BatterySim;
import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
-/**
- * This is a sample program to demonstrate the use of elevator simulation with existing code.
- */
+/** This is a sample program to demonstrate the use of arm simulation with existing code. */
public class Robot extends TimedRobot {
private static final int kMotorPort = 0;
private static final int kEncoderAChannel = 0;
@@ -31,47 +32,67 @@
private static final int kJoystickPort = 0;
// The P gain for the PID controller that drives this arm.
- private static final double kArmKp = 5.0;
+ private static final double kArmKp = 50.0;
// distance per pulse = (angle per revolution) / (pulses per revolution)
// = (2 * PI rads) / (4096 pulses)
private static final double kArmEncoderDistPerPulse = 2.0 * Math.PI / 4096;
- // The arm gearbox represents a gerbox containing two Vex 775pro motors.
+ // The arm gearbox represents a gearbox containing two Vex 775pro motors.
private final DCMotor m_armGearbox = DCMotor.getVex775Pro(2);
- // Standard classes for controlling our elevator
+ // Standard classes for controlling our arm
private final PIDController m_controller = new PIDController(kArmKp, 0, 0);
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
- private final PWMVictorSPX m_motor = new PWMVictorSPX(kMotorPort);
+ private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
private final Joystick m_joystick = new Joystick(kJoystickPort);
// Simulation classes help us simulate what's going on, including gravity.
- private static final double m_armReduction = 100;
+ private static final double m_armReduction = 600;
private static final double m_armMass = 5.0; // Kilograms
private static final double m_armLength = Units.inchesToMeters(30);
- // This arm sim represents an arm that can travel from -180 degrees (rotated straight backwards)
- // to 0 degrees (rotated straight forwards).
- private final SingleJointedArmSim m_armSim = new SingleJointedArmSim(m_armGearbox,
- m_armReduction,
- SingleJointedArmSim.estimateMOI(m_armLength, m_armMass),
- m_armLength,
- Units.degreesToRadians(-180),
- Units.degreesToRadians(0),
- m_armMass,
- true,
- VecBuilder.fill(Units.degreesToRadians(0.5)) // Add noise with a std-dev of 0.5 degrees
- );
+ // This arm sim represents an arm that can travel from -75 degrees (rotated down front)
+ // to 255 degrees (rotated down in the back).
+ private final SingleJointedArmSim m_armSim =
+ new SingleJointedArmSim(
+ m_armGearbox,
+ m_armReduction,
+ SingleJointedArmSim.estimateMOI(m_armLength, m_armMass),
+ m_armLength,
+ Units.degreesToRadians(-75),
+ Units.degreesToRadians(255),
+ m_armMass,
+ true,
+ VecBuilder.fill(kArmEncoderDistPerPulse) // Add noise with a std-dev of 1 tick
+ );
private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
+ // Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm.
+ private final Mechanism2d m_mech2d = new Mechanism2d(60, 60);
+ private final MechanismRoot2d m_armPivot = m_mech2d.getRoot("ArmPivot", 30, 30);
+ private final MechanismLigament2d m_armTower =
+ m_armPivot.append(new MechanismLigament2d("ArmTower", 30, -90));
+ private final MechanismLigament2d m_arm =
+ m_armPivot.append(
+ new MechanismLigament2d(
+ "Arm",
+ 30,
+ Units.radiansToDegrees(m_armSim.getAngleRads()),
+ 6,
+ new Color8Bit(Color.kYellow)));
+
@Override
public void robotInit() {
m_encoder.setDistancePerPulse(kArmEncoderDistPerPulse);
+
+ // Put Mechanism 2d to SmartDashboard
+ SmartDashboard.putData("Arm Sim", m_mech2d);
+ m_armTower.setColor(new Color8Bit(Color.kBlue));
}
@Override
public void simulationPeriodic() {
- // In this method, we update our simulation of what our elevator is doing
+ // In this method, we update our simulation of what our arm is doing
// First, we set our "inputs" (voltages)
m_armSim.setInput(m_motor.get() * RobotController.getBatteryVoltage());
@@ -81,14 +102,18 @@
// Finally, we set our simulated encoder's readings and simulated battery voltage
m_encoderSim.setDistance(m_armSim.getAngleRads());
// SimBattery estimates loaded battery voltages
- RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDrawAmps()));
+ RoboRioSim.setVInVoltage(
+ BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDrawAmps()));
+
+ // Update the Mechanism Arm angle based on the simulated arm angle
+ m_arm.setAngle(Units.radiansToDegrees(m_armSim.getAngleRads()));
}
@Override
public void teleopPeriodic() {
if (m_joystick.getTrigger()) {
- // Here, we run PID control like normal, with a constant setpoint of 30in.
- var pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.degreesToRadians(0));
+ // Here, we run PID control like normal, with a constant setpoint of 75 degrees.
+ var pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.degreesToRadians(75));
m_motor.setVoltage(pidOutput);
} else {
// Otherwise, we disable the motor.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Main.java
index 1c21a4a..cb1423d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.axiscamera;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Robot.java
index ee4b424..65eafaa 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Robot.java
@@ -1,69 +1,64 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.axiscamera;
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.AxisCamera;
+import edu.wpi.first.cscore.CvSink;
+import edu.wpi.first.cscore.CvSource;
+import edu.wpi.first.wpilibj.TimedRobot;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;
-import edu.wpi.cscore.AxisCamera;
-import edu.wpi.cscore.CvSink;
-import edu.wpi.cscore.CvSource;
-import edu.wpi.first.cameraserver.CameraServer;
-import edu.wpi.first.wpilibj.TimedRobot;
-
/**
- * This is a demo program showing the use of OpenCV to do vision processing. The
- * image is acquired from the Axis camera, then a rectangle is put on the image
- * and sent to the dashboard. OpenCV has many methods for different types of
- * processing.
+ * This is a demo program showing the use of OpenCV to do vision processing. The image is acquired
+ * from the Axis camera, then a rectangle is put on the image and sent to the dashboard. OpenCV has
+ * many methods for different types of processing.
*/
public class Robot extends TimedRobot {
Thread m_visionThread;
@Override
public void robotInit() {
- m_visionThread = new Thread(() -> {
- // Get the Axis camera from CameraServer
- AxisCamera camera
- = CameraServer.getInstance().addAxisCamera("axis-camera.local");
- // Set the resolution
- camera.setResolution(640, 480);
+ m_visionThread =
+ new Thread(
+ () -> {
+ // Get the Axis camera from CameraServer
+ AxisCamera camera = CameraServer.addAxisCamera("axis-camera.local");
+ // Set the resolution
+ camera.setResolution(640, 480);
- // Get a CvSink. This will capture Mats from the camera
- CvSink cvSink = CameraServer.getInstance().getVideo();
- // Setup a CvSource. This will send images back to the Dashboard
- CvSource outputStream
- = CameraServer.getInstance().putVideo("Rectangle", 640, 480);
+ // Get a CvSink. This will capture Mats from the camera
+ CvSink cvSink = CameraServer.getVideo();
+ // Setup a CvSource. This will send images back to the Dashboard
+ CvSource outputStream = CameraServer.putVideo("Rectangle", 640, 480);
- // Mats are very memory expensive. Lets reuse this Mat.
- Mat mat = new Mat();
+ // Mats are very memory expensive. Lets reuse this Mat.
+ Mat mat = new Mat();
- // This cannot be 'true'. The program will never exit if it is. This
- // lets the robot stop this thread when restarting robot code or
- // deploying.
- while (!Thread.interrupted()) {
- // Tell the CvSink to grab a frame from the camera and put it
- // in the source mat. If there is an error notify the output.
- if (cvSink.grabFrame(mat) == 0) {
- // Send the output the error.
- outputStream.notifyError(cvSink.getError());
- // skip the rest of the current iteration
- continue;
- }
- // Put a rectangle on the image
- Imgproc.rectangle(mat, new Point(100, 100), new Point(400, 400),
- new Scalar(255, 255, 255), 5);
- // Give the output stream a new image to display
- outputStream.putFrame(mat);
- }
- });
+ // This cannot be 'true'. The program will never exit if it is. This
+ // lets the robot stop this thread when restarting robot code or
+ // deploying.
+ while (!Thread.interrupted()) {
+ // Tell the CvSink to grab a frame from the camera and put it
+ // in the source mat. If there is an error notify the output.
+ if (cvSink.grabFrame(mat) == 0) {
+ // Send the output the error.
+ outputStream.notifyError(cvSink.getError());
+ // skip the rest of the current iteration
+ continue;
+ }
+ // Put a rectangle on the image
+ Imgproc.rectangle(
+ mat, new Point(100, 100), new Point(400, 400), new Scalar(255, 255, 255), 5);
+ // Give the output stream a new image to display
+ outputStream.putFrame(mat);
+ }
+ });
m_visionThread.setDaemon(true);
m_visionThread.start();
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Main.java
index ff5c19c..bafb3af 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.canpdp;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Robot.java
index 7a0831b..1d5e24f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Robot.java
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.canpdp;
-import edu.wpi.first.wpilibj.PowerDistributionPanel;
+import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -16,9 +13,7 @@
* via CAN. The information will be displayed under variables through the SmartDashboard.
*/
public class Robot extends TimedRobot {
- private static final int kPDPId = 0;
-
- private final PowerDistributionPanel m_pdp = new PowerDistributionPanel(kPDPId);
+ private final PowerDistribution m_pdp = new PowerDistribution();
@Override
public void robotPeriodic() {
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 b9c69a8..07b4881 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
@@ -1,27 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.differentialdrivebot;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+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.
- */
+/** Represents a differential drive style drivetrain. */
public class Drivetrain {
public static final double kMaxSpeed = 3.0; // meters per second
public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
@@ -30,26 +25,26 @@
private static final double kWheelRadius = 0.0508; // meters
private static final int kEncoderResolution = 4096;
- private final SpeedController m_leftLeader = new PWMVictorSPX(1);
- private final SpeedController m_leftFollower = new PWMVictorSPX(2);
- private final SpeedController m_rightLeader = new PWMVictorSPX(3);
- private final SpeedController m_rightFollower = new PWMVictorSPX(4);
+ 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 Encoder m_leftEncoder = new Encoder(0, 1);
private final Encoder m_rightEncoder = new Encoder(2, 3);
- private final SpeedControllerGroup m_leftGroup
- = new SpeedControllerGroup(m_leftLeader, m_leftFollower);
- private final SpeedControllerGroup m_rightGroup
- = new SpeedControllerGroup(m_rightLeader, m_rightFollower);
+ 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);
private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
- private final DifferentialDriveKinematics m_kinematics
- = new DifferentialDriveKinematics(kTrackWidth);
+ private final DifferentialDriveKinematics m_kinematics =
+ new DifferentialDriveKinematics(kTrackWidth);
private final DifferentialDriveOdometry m_odometry;
@@ -57,12 +52,17 @@
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
/**
- * Constructs a differential drive object.
- * Sets the encoder distance per pulse and resets the gyro.
+ * Constructs a differential drive object. Sets the encoder distance per pulse and resets the
+ * gyro.
*/
public Drivetrain() {
m_gyro.reset();
+ // 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);
+
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
// resolution.
@@ -84,10 +84,10 @@
final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
- final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(),
- speeds.leftMetersPerSecond);
- final double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(),
- speeds.rightMetersPerSecond);
+ final double leftOutput =
+ 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);
}
@@ -96,7 +96,7 @@
* Drives the robot with the given linear velocity and angular velocity.
*
* @param xSpeed Linear velocity in m/s.
- * @param rot Angular velocity in rad/s.
+ * @param rot Angular velocity in rad/s.
*/
@SuppressWarnings("ParameterName")
public void drive(double xSpeed, double rot) {
@@ -104,11 +104,9 @@
setSpeeds(wheelSpeeds);
}
- /**
- * Updates the field-relative position.
- */
+ /** Updates the field-relative position. */
public void updateOdometry() {
- m_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(),
- m_rightEncoder.getDistance());
+ m_odometry.update(
+ m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Main.java
index 1f2b319..89f8dae 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.differentialdrivebot;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java
index 6e7416d..8cd5022 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java
@@ -1,14 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.differentialdrivebot;
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.SlewRateLimiter;
+import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
@@ -20,7 +16,6 @@
private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
-
@Override
public void autonomousPeriodic() {
teleopPeriodic();
@@ -31,17 +26,13 @@
public void teleopPeriodic() {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
- final var xSpeed =
- -m_speedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
- * Drivetrain.kMaxSpeed;
+ final var xSpeed = -m_speedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
- final var rot =
- -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
- * Drivetrain.kMaxAngularSpeed;
+ final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
m_drive.drive(xSpeed, rot);
}
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
new file mode 100644
index 0000000..3b5d9e2
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.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.differentialdriveposeestimator;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.util.Units;
+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.MotorControllerGroup;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+/** Represents a differential drive style drivetrain. */
+public class Drivetrain {
+ public static final double kMaxSpeed = 3.0; // meters per second
+ public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
+
+ private static final double kTrackWidth = 0.381 * 2; // meters
+ private static final double kWheelRadius = 0.0508; // meters
+ private static final int kEncoderResolution = 4096;
+
+ private final 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 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);
+ private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
+
+ private final DifferentialDriveKinematics m_kinematics =
+ new DifferentialDriveKinematics(kTrackWidth);
+
+ /* Here we use DifferentialDrivePoseEstimator so that we can fuse odometry readings. The
+ numbers used below are robot specific, and should be tuned. */
+ private final DifferentialDrivePoseEstimator m_poseEstimator =
+ new DifferentialDrivePoseEstimator(
+ m_gyro.getRotation2d(),
+ new Pose2d(),
+ VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.01, 0.01),
+ VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(1)),
+ VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+
+ // Gains are for example purposes only - must be determined for your own robot!
+ private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
+
+ /**
+ * Constructs a differential drive object. Sets the encoder distance per pulse and resets the
+ * gyro.
+ */
+ public Drivetrain() {
+ m_gyro.reset();
+
+ // 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);
+
+ // Set the distance per pulse for the drive encoders. We can simply use the
+ // distance traveled for one rotation of the wheel divided by the encoder
+ // resolution.
+ m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+ m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+ }
+
+ /**
+ * Sets the desired wheel speeds.
+ *
+ * @param speeds The desired wheel speeds.
+ */
+ public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
+ final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
+ final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
+
+ final double leftOutput =
+ 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);
+ }
+
+ /**
+ * Drives the robot with the given linear velocity and angular velocity.
+ *
+ * @param xSpeed Linear velocity in m/s.
+ * @param rot Angular velocity in rad/s.
+ */
+ @SuppressWarnings("ParameterName")
+ public void drive(double xSpeed, double rot) {
+ var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot));
+ setSpeeds(wheelSpeeds);
+ }
+
+ /** Updates the field-relative position. */
+ public void updateOdometry() {
+ m_poseEstimator.update(
+ m_gyro.getRotation2d(),
+ new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()),
+ m_leftEncoder.getDistance(),
+ m_rightEncoder.getDistance());
+
+ // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
+ // a real robot, this must be calculated based either on latency or timestamps.
+ m_poseEstimator.addVisionMeasurement(
+ ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
+ m_poseEstimator.getEstimatedPosition()),
+ Timer.getFPGATimestamp() - 0.3);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/ExampleGlobalMeasurementSensor.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/ExampleGlobalMeasurementSensor.java
new file mode 100644
index 0000000..4697bba
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/ExampleGlobalMeasurementSensor.java
@@ -0,0 +1,32 @@
+// 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.differentialdriveposeestimator;
+
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+
+/** This dummy class represents a global measurement sensor, such as a computer vision solution. */
+public final class ExampleGlobalMeasurementSensor {
+ private ExampleGlobalMeasurementSensor() {
+ // Utility class
+ }
+
+ /**
+ * Get a "noisy" fake global pose reading.
+ *
+ * @param estimatedRobotPose The robot pose.
+ */
+ public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
+ var rand =
+ StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+ return new Pose2d(
+ estimatedRobotPose.getX() + rand.get(0, 0),
+ estimatedRobotPose.getY() + rand.get(1, 0),
+ estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0))));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Main.java
new file mode 100644
index 0000000..046c01a
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/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.differentialdriveposeestimator;
+
+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/differentialdriveposeestimator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java
new file mode 100644
index 0000000..1011724
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java
@@ -0,0 +1,39 @@
+// 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.differentialdriveposeestimator;
+
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class Robot extends TimedRobot {
+ private final XboxController m_controller = new XboxController(0);
+ private final Drivetrain m_drive = new Drivetrain();
+
+ // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+ private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
+ private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
+ @Override
+ public void autonomousPeriodic() {
+ teleopPeriodic();
+ m_drive.updateOdometry();
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ // Get the x speed. We are inverting this because Xbox controllers return
+ // negative values when we push forward.
+ final var xSpeed = -m_speedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
+
+ // Get the rate of angular rotation. We are inverting this because we want a
+ // positive value when we pull to the left (remember, CCW is positive in
+ // mathematics). Xbox controllers return positive values when you pull to
+ // the right by default.
+ final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
+
+ m_drive.drive(xSpeed, rot);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/Main.java
new file mode 100644
index 0000000..97f67d1
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/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.dma;
+
+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/dma/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/Robot.java
new file mode 100644
index 0000000..a7b088c
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/Robot.java
@@ -0,0 +1,85 @@
+// 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.dma;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.DMA;
+import edu.wpi.first.wpilibj.DMASample;
+import edu.wpi.first.wpilibj.DigitalOutput;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/** This is a sample program showing how to to use DMA to read a sensor. */
+public class Robot extends TimedRobot {
+ private DMA m_dma;
+ private DMASample m_dmaSample;
+
+ // DMA needs a trigger, can use an output as trigger.
+ // 8 Triggers exist per DMA object, can be triggered on any
+ // DigitalSource.
+ private DigitalOutput m_dmaTrigger;
+
+ // Analog input to read with DMA
+ private AnalogInput m_analogInput;
+
+ // Encoder to read with DMA
+ private Encoder m_encoder;
+
+ @Override
+ public void robotInit() {
+ m_dma = new DMA();
+ m_dmaSample = new DMASample();
+ m_dmaTrigger = new DigitalOutput(2);
+ m_analogInput = new AnalogInput(0);
+ m_encoder = new Encoder(0, 1);
+
+ // Trigger on falling edge of dma trigger output
+ m_dma.setExternalTrigger(m_dmaTrigger, false, true);
+
+ // Add inputs we want to read via DMA
+ m_dma.addAnalogInput(m_analogInput);
+ m_dma.addEncoder(m_encoder);
+ m_dma.addEncoderPeriod(m_encoder);
+
+ // Make sure trigger is set to off.
+ m_dmaTrigger.set(true);
+
+ // Start DMA. No triggers or inputs can be added after this call
+ // unless DMA is stopped.
+ m_dma.start(1024);
+ }
+
+ @Override
+ public void robotPeriodic() {
+ // Manually Trigger DMA read
+ m_dmaTrigger.set(false);
+
+ // Update our sample. remaining is the number of samples remaining in the
+ // buffer status is more specific error messages if readStatus is not OK.
+ // Wait 1ms if buffer is empty
+ DMASample.DMAReadStatus readStatus = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(1));
+
+ // Unset trigger
+ m_dmaTrigger.set(true);
+
+ if (readStatus == DMASample.DMAReadStatus.kOk) {
+ // Status value in all these reads should be checked, a non 0 value means
+ // value could not be read
+
+ // If DMA is good, values exist
+ double encoderDistance = m_dmaSample.getEncoderDistance(m_encoder);
+ // Period is not scaled, and is a raw value
+ int encoderPeriod = m_dmaSample.getEncoderPeriodRaw(m_encoder);
+ double analogVoltage = m_dmaSample.getAnalogInputVoltage(m_analogInput);
+
+ SmartDashboard.putNumber("Distance", encoderDistance);
+ SmartDashboard.putNumber("Period", encoderPeriod);
+ SmartDashboard.putNumber("Input", analogVoltage);
+ SmartDashboard.putNumber("Timestamp", m_dmaSample.getTimeStamp());
+ }
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java
index 5f4bbeb..64ba267 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.drivedistanceoffboard;
/**
* 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.
+ * 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.
@@ -38,6 +35,6 @@
}
public static final class OIConstants {
- public static final int kDriverControllerPort = 1;
+ public static final int kDriverControllerPort = 0;
}
}
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 e2fb7c8..487974c 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
@@ -1,34 +1,32 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.drivedistanceoffboard;
-import edu.wpi.first.wpilibj.SpeedController;
+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 SpeedController {
+public class ExampleSmartMotorController implements MotorController {
public enum PIDMode {
kPosition,
kVelocity,
kMovementWitchcraft
}
+ double m_value;
+
/**
* Creates a new ExampleSmartMotorController.
*
* @param port The port for the controller.
*/
@SuppressWarnings("PMD.UnusedFormalParameter")
- public ExampleSmartMotorController(int port) {
- }
+ public ExampleSmartMotorController(int port) {}
/**
* Example method for setting the PID gains of the smart controller.
@@ -37,8 +35,7 @@
* @param ki The integral gain.
* @param kd The derivative gain.
*/
- public void setPID(double kp, double ki, double kd) {
- }
+ public void setPID(double kp, double ki, double kd) {}
/**
* Example method for setting the setpoint of the smart controller in PID mode.
@@ -47,16 +44,14 @@
* @param setpoint The controller setpoint.
* @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
*/
- public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {
- }
+ public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
/**
* Places this motor controller in follower mode.
*
* @param leader The leader to follow.
*/
- public void follow(ExampleSmartMotorController leader) {
- }
+ public void follow(ExampleSmartMotorController leader) {}
/**
* Returns the encoder distance.
@@ -76,25 +71,21 @@
return 0;
}
- /**
- * Resets the encoder to zero distance.
- */
- public void resetEncoder() {
- }
+ /** Resets the encoder to zero distance. */
+ public void resetEncoder() {}
@Override
public void set(double speed) {
+ m_value = speed;
}
@Override
public double get() {
- return 0;
+ return m_value;
}
@Override
- public void setInverted(boolean isInverted) {
-
- }
+ public void setInverted(boolean isInverted) {}
@Override
public boolean getInverted() {
@@ -102,14 +93,8 @@
}
@Override
- public void disable() {
- }
+ public void disable() {}
@Override
- public void stopMotor() {
- }
-
- @Override
- public void pidWrite(double output) {
- }
+ public void stopMotor() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java
index 89ab553..3566c19 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.drivedistanceoffboard;
@@ -15,8 +12,7 @@
* call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java
index e8ac285..87c9d52 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.drivedistanceoffboard;
@@ -37,8 +34,8 @@
* 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.
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
@@ -49,20 +46,14 @@
CommandScheduler.getInstance().run();
}
- /**
- * This function is called once each time the robot enters Disabled mode.
- */
+ /** This function is called once each time the robot enters Disabled mode. */
@Override
- public void disabledInit() {
- }
+ public void disabledInit() {}
@Override
- public void disabledPeriodic() {
- }
+ public void disabledPeriodic() {}
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
}
}
- /**
- * This function is called periodically during autonomous.
- */
+ /** This function is called periodically during autonomous. */
@Override
- public void autonomousPeriodic() {
- }
+ public void autonomousPeriodic() {}
@Override
public void teleopInit() {
@@ -98,13 +86,9 @@
}
}
- /**
- * This function is called periodically during operator control.
- */
+ /** This function is called periodically during operator control. */
@Override
- public void teleopPeriodic() {
-
- }
+ public void teleopPeriodic() {}
@Override
public void testInit() {
@@ -112,10 +96,7 @@
CommandScheduler.getInstance().cancelAll();
}
- /**
- * This function is called periodically during test mode.
- */
+ /** This function is called periodically during test mode. */
@Override
- public void testPeriodic() {
- }
+ public void testPeriodic() {}
}
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 f618b9d..78b7f81 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
@@ -1,33 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.drivedistanceoffboard;
-import edu.wpi.first.wpilibj.GenericHID;
+import static edu.wpi.first.wpilibj.XboxController.Button;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.OIConstants;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands.DriveDistanceProfiled;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.OIConstants;
-import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands.DriveDistanceProfiled;
-import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
-
/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * 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.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems
@@ -36,9 +31,7 @@
// The driver's controller
XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
- /**
- * The container for the robot. Contains subsystems, OI devices, and commands.
- */
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
@@ -48,17 +41,18 @@
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
- new RunCommand(() -> m_robotDrive
- .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
- m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
-
+ new RunCommand(
+ () ->
+ m_robotDrive.arcadeDrive(
+ m_driverController.getLeftY(), m_driverController.getRightX()),
+ m_robotDrive));
}
/**
- * Use this method to define your button->command mappings. Buttons can be created by
- * instantiating a {@link GenericHID} or one of its subclasses ({@link
- * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
- * {@link JoystickButton}.
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+ * JoystickButton}.
*/
private void configureButtonBindings() {
// Drive forward by 3 meters when the 'A' button is pressed, with a timeout of 10 seconds
@@ -69,29 +63,26 @@
new JoystickButton(m_driverController, Button.kB.value)
.whenPressed(
new TrapezoidProfileCommand(
- new TrapezoidProfile(
- // Limit the max acceleration and velocity
- new TrapezoidProfile.Constraints(
- DriveConstants.kMaxSpeedMetersPerSecond,
- DriveConstants.kMaxAccelerationMetersPerSecondSquared),
- // End at desired position in meters; implicitly starts at 0
- new TrapezoidProfile.State(3, 0)),
- // Pipe the profile state to the drive
- setpointState -> m_robotDrive.setDriveStates(
- setpointState,
- setpointState),
- // Require the drive
- m_robotDrive)
+ new TrapezoidProfile(
+ // Limit the max acceleration and velocity
+ new TrapezoidProfile.Constraints(
+ DriveConstants.kMaxSpeedMetersPerSecond,
+ DriveConstants.kMaxAccelerationMetersPerSecondSquared),
+ // End at desired position in meters; implicitly starts at 0
+ new TrapezoidProfile.State(3, 0)),
+ // Pipe the profile state to the drive
+ setpointState -> m_robotDrive.setDriveStates(setpointState, setpointState),
+ // Require the drive
+ m_robotDrive)
.beforeStarting(m_robotDrive::resetEncoders)
.withTimeout(10));
// Drive at half speed when the bumper is held
- new JoystickButton(m_driverController, Button.kBumperRight.value)
+ new JoystickButton(m_driverController, Button.kRightBumper.value)
.whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
}
-
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
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 280acd3..5bfa10d 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
@@ -1,21 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.drivedistanceoffboard.commands;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
-
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
-/**
- * Drives a set distance using a motion profile.
- */
+/** Drives a set distance using a motion profile. */
public class DriveDistanceProfiled extends TrapezoidProfileCommand {
/**
* Creates a new DriveDistanceProfiled command.
@@ -27,8 +21,9 @@
super(
new TrapezoidProfile(
// Limit the max acceleration and velocity
- new TrapezoidProfile.Constraints(DriveConstants.kMaxSpeedMetersPerSecond,
- DriveConstants.kMaxAccelerationMetersPerSecondSquared),
+ new TrapezoidProfile.Constraints(
+ DriveConstants.kMaxSpeedMetersPerSecond,
+ DriveConstants.kMaxAccelerationMetersPerSecondSquared),
// End at desired position in meters; implicitly starts at 0
new TrapezoidProfile.State(meters, 0)),
// Pipe the profile state to the drive
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 3d52adb..124c1fe 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
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.drivedistanceoffboard.subsystems;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.ExampleSmartMotorController;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
@@ -30,17 +26,26 @@
new ExampleSmartMotorController(DriveConstants.kRightMotor2Port);
private final SimpleMotorFeedforward m_feedforward =
- new SimpleMotorFeedforward(DriveConstants.ksVolts,
- DriveConstants.kvVoltSecondsPerMeter,
- DriveConstants.kaVoltSecondsSquaredPerMeter);
+ new SimpleMotorFeedforward(
+ DriveConstants.ksVolts,
+ DriveConstants.kvVoltSecondsPerMeter,
+ DriveConstants.kaVoltSecondsSquaredPerMeter);
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftLeader, m_rightLeader);
- /**
- * Creates a new DriveSubsystem.
- */
+ /** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ // 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);
@@ -65,12 +70,14 @@
* @param right The right wheel state.
*/
public void setDriveStates(TrapezoidProfile.State left, TrapezoidProfile.State right) {
- m_leftLeader.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition,
- left.position,
- m_feedforward.calculate(left.velocity));
- m_rightLeader.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition,
- right.position,
- m_feedforward.calculate(right.velocity));
+ m_leftLeader.setSetpoint(
+ ExampleSmartMotorController.PIDMode.kPosition,
+ left.position,
+ m_feedforward.calculate(left.velocity));
+ m_rightLeader.setSetpoint(
+ ExampleSmartMotorController.PIDMode.kPosition,
+ right.position,
+ m_feedforward.calculate(right.velocity));
}
/**
@@ -91,16 +98,14 @@
return m_rightLeader.getEncoderDistance();
}
- /**
- * Resets the drive encoders.
- */
+ /** Resets the drive encoders. */
public void resetEncoders() {
m_leftLeader.resetEncoder();
m_rightLeader.resetEncoder();
}
/**
- * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Main.java
index a051005..1ba73f5 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Main.java
@@ -1,29 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.dutycycleencoder;
-
-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);
- }
-}
+// 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.dutycycleencoder;
+
+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/dutycycleencoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java
index 5755f72..a12a6fd 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java
@@ -1,46 +1,42 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.dutycycleencoder;
-
-import edu.wpi.first.wpilibj.DutyCycleEncoder;
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-@SuppressWarnings({"PMD.SingularField"})
-public class Robot extends TimedRobot {
- private DutyCycleEncoder m_dutyCycleEncoder;
-
- @Override
- public void robotInit() {
- m_dutyCycleEncoder = new DutyCycleEncoder(0);
-
- // Set to 0.5 units per rotation
- m_dutyCycleEncoder.setDistancePerRotation(0.5);
- }
-
- @Override
- public void robotPeriodic() {
- // Connected can be checked, and uses the frequency of the encoder
- boolean connected = m_dutyCycleEncoder.isConnected();
-
- // Duty Cycle Frequency in Hz
- int frequency = m_dutyCycleEncoder.getFrequency();
-
- // Output of encoder
- double output = m_dutyCycleEncoder.get();
-
- // Output scaled by DistancePerPulse
- double distance = m_dutyCycleEncoder.getDistance();
-
- SmartDashboard.putBoolean("Connected", connected);
- SmartDashboard.putNumber("Frequency", frequency);
- SmartDashboard.putNumber("Output", output);
- SmartDashboard.putNumber("Distance", distance);
- }
-
-}
+// 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.dutycycleencoder;
+
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+@SuppressWarnings("PMD.SingularField")
+public class Robot extends TimedRobot {
+ private DutyCycleEncoder m_dutyCycleEncoder;
+
+ @Override
+ public void robotInit() {
+ m_dutyCycleEncoder = new DutyCycleEncoder(0);
+
+ // Set to 0.5 units per rotation
+ m_dutyCycleEncoder.setDistancePerRotation(0.5);
+ }
+
+ @Override
+ public void robotPeriodic() {
+ // Connected can be checked, and uses the frequency of the encoder
+ boolean connected = m_dutyCycleEncoder.isConnected();
+
+ // Duty Cycle Frequency in Hz
+ int frequency = m_dutyCycleEncoder.getFrequency();
+
+ // Output of encoder
+ double output = m_dutyCycleEncoder.get();
+
+ // Output scaled by DistancePerPulse
+ double distance = m_dutyCycleEncoder.getDistance();
+
+ SmartDashboard.putBoolean("Connected", connected);
+ SmartDashboard.putNumber("Frequency", frequency);
+ SmartDashboard.putNumber("Output", output);
+ SmartDashboard.putNumber("Distance", distance);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Main.java
index a7cbfc0..4558281 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Main.java
@@ -1,29 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.dutycycleinput;
-
-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);
- }
-}
+// 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.dutycycleinput;
+
+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/dutycycleinput/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Robot.java
index 657dc9b..584fbc6 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Robot.java
@@ -1,39 +1,35 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.dutycycleinput;
-
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.DutyCycle;
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-@SuppressWarnings({"PMD.SingularField"})
-public class Robot extends TimedRobot {
- private DigitalInput m_input;
- private DutyCycle m_dutyCycle;
-
- @Override
- public void robotInit() {
- m_input = new DigitalInput(0);
- m_dutyCycle = new DutyCycle(m_input);
- }
-
- @Override
- public void robotPeriodic() {
- // Duty Cycle Frequency in Hz
- int frequency = m_dutyCycle.getFrequency();
-
- // Output of duty cycle, ranging from 0 to 1
- // 1 is fully on, 0 is fully off
- double output = m_dutyCycle.getOutput();
-
- SmartDashboard.putNumber("Frequency", frequency);
- SmartDashboard.putNumber("Duty Cycle", output);
- }
-
-}
+// 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.dutycycleinput;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DutyCycle;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+@SuppressWarnings("PMD.SingularField")
+public class Robot extends TimedRobot {
+ private DigitalInput m_input;
+ private DutyCycle m_dutyCycle;
+
+ @Override
+ public void robotInit() {
+ m_input = new DigitalInput(0);
+ m_dutyCycle = new DutyCycle(m_input);
+ }
+
+ @Override
+ public void robotPeriodic() {
+ // Duty Cycle Frequency in Hz
+ int frequency = m_dutyCycle.getFrequency();
+
+ // Output of duty cycle, ranging from 0 to 1
+ // 1 is fully on, 0 is fully off
+ double output = m_dutyCycle.getOutput();
+
+ SmartDashboard.putNumber("Frequency", frequency);
+ SmartDashboard.putNumber("Duty Cycle", output);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Main.java
index 5df92a6..223b40d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.elevatorprofiledpid;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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 5d3d546..15d5e23 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
@@ -1,26 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.elevatorprofiledpid;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
public class Robot extends TimedRobot {
private static double kDt = 0.02;
private final Joystick m_joystick = new Joystick(1);
private final Encoder m_encoder = new Encoder(1, 2);
- private final SpeedController m_motor = new PWMVictorSPX(1);
+ private final MotorController 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/elevatorsimulation/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Main.java
index 22cde2e..ab930d3 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.elevatorsimulation;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java
index 600437a..3f2bae8 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java
@@ -1,29 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.elevatorsimulation;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.simulation.EncoderSim;
-import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.simulation.BatterySim;
import edu.wpi.first.wpilibj.simulation.ElevatorSim;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-/**
- * This is a sample program to demonstrate the use of elevator simulation with existing code.
- */
+/** This is a sample program to demonstrate the use of elevator simulation with existing code. */
public class Robot extends TimedRobot {
private static final int kMotorPort = 0;
private static final int kEncoderAChannel = 0;
@@ -35,34 +34,48 @@
private static final double kElevatorDrumRadius = Units.inchesToMeters(2.0);
private static final double kCarriageMass = 4.0; // kg
- private static final double kMinElevatorHeight = 0.0;
+ private static final double kMinElevatorHeight = Units.inchesToMeters(2);
private static final double kMaxElevatorHeight = Units.inchesToMeters(50);
// distance per pulse = (distance per revolution) / (pulses per revolution)
// = (Pi * D) / ppr
- private static final double kElevatorEncoderDistPerPulse = 2.0 * Math.PI * kElevatorDrumRadius / 4096;
+ private static final double kElevatorEncoderDistPerPulse =
+ 2.0 * Math.PI * kElevatorDrumRadius / 4096;
private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4);
// Standard classes for controlling our elevator
private final PIDController m_controller = new PIDController(kElevatorKp, 0, 0);
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
- private final PWMVictorSPX m_motor = new PWMVictorSPX(kMotorPort);
+ private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
private final Joystick m_joystick = new Joystick(kJoystickPort);
// Simulation classes help us simulate what's going on, including gravity.
- private final ElevatorSim m_elevatorSim = new ElevatorSim(m_elevatorGearbox,
- kElevatorGearing,
- kCarriageMass,
- kElevatorDrumRadius,
- kMinElevatorHeight,
- kMaxElevatorHeight,
- VecBuilder.fill(0.01));
+ private final ElevatorSim m_elevatorSim =
+ new ElevatorSim(
+ m_elevatorGearbox,
+ kElevatorGearing,
+ kCarriageMass,
+ kElevatorDrumRadius,
+ kMinElevatorHeight,
+ kMaxElevatorHeight,
+ VecBuilder.fill(0.01));
private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
+ // Create a Mechanism2d visualization of the elevator
+ private final Mechanism2d m_mech2d = new Mechanism2d(20, 50);
+ private final MechanismRoot2d m_mech2dRoot = m_mech2d.getRoot("Elevator Root", 10, 0);
+ private final MechanismLigament2d m_elevatorMech2d =
+ m_mech2dRoot.append(
+ new MechanismLigament2d(
+ "Elevator", Units.metersToInches(m_elevatorSim.getPositionMeters()), 90));
+
@Override
public void robotInit() {
m_encoder.setDistancePerPulse(kElevatorEncoderDistPerPulse);
+
+ // Publish Mechanism2d to SmartDashboard
+ SmartDashboard.putData("Elevator Sim", m_mech2d);
}
@Override
@@ -77,7 +90,11 @@
// Finally, we set our simulated encoder's readings and simulated battery voltage
m_encoderSim.setDistance(m_elevatorSim.getPositionMeters());
// SimBattery estimates loaded battery voltages
- RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
+ RoboRioSim.setVInVoltage(
+ BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
+
+ // Update elevator visualization with simulated position
+ m_elevatorMech2d.setLength(Units.metersToInches(m_elevatorSim.getPositionMeters()));
}
@Override
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 94e3c77..a1366e6 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
@@ -1,20 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.elevatortrapezoidprofile;
-import edu.wpi.first.wpilibj.SpeedController;
+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 SpeedController {
+public class ExampleSmartMotorController implements MotorController {
public enum PIDMode {
kPosition,
kVelocity,
@@ -27,8 +24,7 @@
* @param port The port for the controller.
*/
@SuppressWarnings("PMD.UnusedFormalParameter")
- public ExampleSmartMotorController(int port) {
- }
+ public ExampleSmartMotorController(int port) {}
/**
* Example method for setting the PID gains of the smart controller.
@@ -37,8 +33,7 @@
* @param ki The integral gain.
* @param kd The derivative gain.
*/
- public void setPID(double kp, double ki, double kd) {
- }
+ public void setPID(double kp, double ki, double kd) {}
/**
* Example method for setting the setpoint of the smart controller in PID mode.
@@ -47,16 +42,14 @@
* @param setpoint The controller setpoint.
* @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
*/
- public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {
- }
+ public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
/**
* Places this motor controller in follower mode.
*
* @param leader The leader to follow.
*/
- public void follow(ExampleSmartMotorController leader) {
- }
+ public void follow(ExampleSmartMotorController leader) {}
/**
* Returns the encoder distance.
@@ -76,15 +69,11 @@
return 0;
}
- /**
- * Resets the encoder to zero distance.
- */
- public void resetEncoder() {
- }
+ /** Resets the encoder to zero distance. */
+ public void resetEncoder() {}
@Override
- public void set(double speed) {
- }
+ public void set(double speed) {}
@Override
public double get() {
@@ -92,9 +81,7 @@
}
@Override
- public void setInverted(boolean isInverted) {
-
- }
+ public void setInverted(boolean isInverted) {}
@Override
public boolean getInverted() {
@@ -102,14 +89,8 @@
}
@Override
- public void disable() {
- }
+ public void disable() {}
@Override
- public void stopMotor() {
- }
-
- @Override
- public void pidWrite(double output) {
- }
+ public void stopMotor() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Main.java
index 12255cd..5428143 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.elevatortrapezoidprofile;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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 918d3d3..d459eeb 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
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.elevatortrapezoidprofile;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
public class Robot extends TimedRobot {
private static double kDt = 0.02;
@@ -49,7 +46,9 @@
m_setpoint = profile.calculate(kDt);
// Send setpoint to offboard controller PID
- m_motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, m_setpoint.position,
- m_feedforward.calculate(m_setpoint.velocity) / 12.0);
+ m_motor.setSetpoint(
+ ExampleSmartMotorController.PIDMode.kPosition,
+ m_setpoint.position,
+ m_feedforward.calculate(m_setpoint.velocity) / 12.0);
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Main.java
index da0be79..4f723cc 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.encoder;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java
index ef34fe5..3b715eb 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.encoder;
@@ -29,14 +26,13 @@
* The Encoder object is constructed with 4 parameters, the last two being optional. The first two
* parameters (1, 2 in this case) refer to the ports on the roboRIO which the encoder uses.
* Because a quadrature encoder has two signal wires, the signal from two DIO ports on the roboRIO
- * are used. The third (optional) parameter is a boolean which defaults to false. If you set this
- * parameter to true, the direction of the encoder will be reversed, in case it makes more sense
+ * are used. The third (optional) parameter is a boolean which defaults to false. If you set this
+ * parameter to true, the direction of the encoder will be reversed, in case it makes more sense
* mechanically. The final (optional) parameter specifies encoding rate (k1X, k2X, or k4X) and
* defaults to k4X. Faster (k4X) encoding gives greater positional precision but more noise in the
* rate.
*/
- private final Encoder m_encoder =
- new Encoder(1, 2, false, CounterBase.EncodingType.k4X);
+ private final Encoder m_encoder = new Encoder(1, 2, false, CounterBase.EncodingType.k4X);
@Override
public void robotInit() {
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 5bc23ab..c270fc4 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
@@ -12,7 +12,7 @@
},
{
"name": "Tank Drive",
- "description": "Demonstrate the use of the RobotDrive class doing teleop driving with tank steering",
+ "description": "Demonstrate the use of the DifferentialDrive class doing teleop driving with tank steering",
"tags": [
"Actuators",
"Joystick",
@@ -37,7 +37,7 @@
},
{
"name": "Mecanum Drive",
- "description": "Demonstrate the use of the RobotDrive class doing teleop driving with Mecanum steering",
+ "description": "Demonstrate the use of the MecanumDrive class doing teleop driving with Mecanum steering",
"tags": [
"Actuators",
"Joystick",
@@ -209,6 +209,14 @@
"mainclass": "Main",
"commandversion": 2
},
+ {"name": "Mechanism2d",
+ "description": "An example usage of Mechanism2d to display mechanism states on a dashboard.",
+ "tags": ["Mechanism2d"],
+ "foldername": "mechanism2d",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
{
"name": "Motor Controller",
"description": "Demonstrate controlling a single motor with a joystick",
@@ -250,17 +258,6 @@
"commandversion": 2
},
{
- "name": "PacGoat",
- "description": "A fully functional example CommandBased program for FRC Team 190's 2014 robot. This code can run on your computer if it supports simulation.",
- "tags": [
- "Complete Robot"
- ],
- "foldername": "pacgoat",
- "gradlebase": "java",
- "mainclass": "Main",
- "commandversion": 1
- },
- {
"name": "Simple Vision",
"description": "Demonstrate the use of the CameraServer class to stream from a USB Webcam without processing the images.",
"tags": [
@@ -274,7 +271,7 @@
},
{
"name": "Intermediate Vision",
- "description": "Demonstrate the use of the NIVision class to capture image from a Webcam, process them, and then send them to the dashboard.",
+ "description": "An example program that acquires images from an attached USB camera and adds some annotation to the image as you might do for showing operators the result of some image recognition, and sends it to the dashboard for display.",
"tags": [
"Vision",
"Complete List"
@@ -484,6 +481,17 @@
"commandversion": 2
},
{
+ "name": "DMA",
+ "description": "Demonstrates the use of the DMA class",
+ "tags": [
+ "Advanced Java"
+ ],
+ "foldername": "dma",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
"name": "ArmBot",
"description": "An example command-based robot demonstrating the use of a ProfiledPIDSubsystem to control an arm.",
"tags": [
@@ -633,6 +641,23 @@
"commandversion": 2
},
{
+ "name": "SimpleDifferentialDriveSimulation",
+ "description": "An example of a minimal drivetrain simulation project without the command-based library.",
+ "tags": [
+ "Drivetrain",
+ "State space",
+ "Digital",
+ "Sensors",
+ "Actuators",
+ "Joystick",
+ "Simulation"
+ ],
+ "foldername": "simpledifferentialdrivesimulation",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
"name": "StateSpaceDriveSimulation",
"description": "An example of drivetrain simulation in combination with a RAMSETE path following controller and the Field2d class.",
"tags": [
@@ -658,7 +683,8 @@
"Digital",
"Sensors",
"Simulation",
- "Physics"
+ "Physics",
+ "Mechanism2d"
],
"foldername": "elevatorsimulation",
"gradlebase": "java",
@@ -674,11 +700,75 @@
"Digital",
"Sensors",
"Simulation",
- "Physics"
+ "Physics",
+ "Mechanism2d"
],
"foldername": "armsimulation",
"gradlebase": "java",
"mainclass": "Main",
"commandversion": 2
+ },
+ {
+ "name": "DifferentialDrivePoseEstimator",
+ "description": "Demonstrates the use of the DifferentialDrivePoseEstimator as a replacement for differential drive odometry.",
+ "tags": [
+ "Drivetrain",
+ "State space",
+ "Vision",
+ "Filter",
+ "Odometry",
+ "Pose",
+ "Differential drive"
+ ],
+ "foldername": "differentialdriveposeestimator",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "MecanumDrivePoseEstimator",
+ "description": "Demonstrates the use of the MecanumDrivePoseEstimator as a replacement for mecanum drive odometry.",
+ "tags": [
+ "Drivetrain",
+ "State space",
+ "Vision",
+ "Filter",
+ "Odometry",
+ "Pose",
+ "Mecanum"
+ ],
+ "foldername": "mecanumdriveposeestimator",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "SwerveDrivePoseEstimator",
+ "description": "Demonstrates the use of the SwerveDrivePoseEstimator as a replacement for swerve drive odometry.",
+ "tags": [
+ "Drivetrain",
+ "State space",
+ "Vision",
+ "Filter",
+ "Odometry",
+ "Pose",
+ "Swerve"
+ ],
+ "foldername": "swervedriveposeestimator",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "RomiReference",
+ "description": "An example command-based robot program that can be used with the Romi reference robot design.",
+ "tags": [
+ "Drivetrain",
+ "Romi"
+ ],
+ "foldername": "romireference",
+ "gradlebase": "javaromi",
+ "mainclass": "Main",
+ "commandversion": 2
}
]
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
index c8c422c..3fe16cf 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.frisbeebot;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants. This class should not be used for any other purpose. All constants should be
- * declared globally (i.e. public static). Do not put anything functional in this class.
+ * 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.
@@ -22,8 +19,8 @@
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
- public static final int[] kLeftEncoderPorts = new int[]{0, 1};
- public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+ public static final int[] kRightEncoderPorts = new int[] {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
@@ -35,7 +32,7 @@
}
public static final class ShooterConstants {
- public static final int[] kEncoderPorts = new int[]{4, 5};
+ public static final int[] kEncoderPorts = new int[] {4, 5};
public static final boolean kEncoderReversed = false;
public static final int kEncoderCPR = 1024;
public static final double kEncoderDistancePerPulse =
@@ -70,6 +67,6 @@
}
public static final class OIConstants {
- public static final int kDriverControllerPort = 1;
+ public static final int kDriverControllerPort = 0;
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java
index 1fdb348..48c3f40 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.frisbeebot;
@@ -15,8 +12,7 @@
* call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java
index 83a289d..a6b0023 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.frisbeebot;
@@ -37,8 +34,8 @@
* 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.
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
@@ -49,20 +46,14 @@
CommandScheduler.getInstance().run();
}
- /**
- * This function is called once each time the robot enters Disabled mode.
- */
+ /** This function is called once each time the robot enters Disabled mode. */
@Override
- public void disabledInit() {
- }
+ public void disabledInit() {}
@Override
- public void disabledPeriodic() {
- }
+ public void disabledPeriodic() {}
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
}
}
- /**
- * This function is called periodically during autonomous.
- */
+ /** This function is called periodically during autonomous. */
@Override
- public void autonomousPeriodic() {
- }
+ public void autonomousPeriodic() {}
@Override
public void teleopInit() {
@@ -98,13 +86,9 @@
}
}
- /**
- * This function is called periodically during operator control.
- */
+ /** This function is called periodically during operator control. */
@Override
- public void teleopPeriodic() {
-
- }
+ public void teleopPeriodic() {}
@Override
public void testInit() {
@@ -112,10 +96,7 @@
CommandScheduler.getInstance().cancelAll();
}
- /**
- * This function is called periodically during test mode.
- */
+ /** This function is called periodically during test mode. */
@Override
- public void testPeriodic() {
- }
+ public void testPeriodic() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
index 957f11f..5d228d0 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
@@ -1,14 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.frisbeebot;
-import edu.wpi.first.wpilibj.GenericHID;
+import static edu.wpi.first.wpilibj.XboxController.Button;
+
import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants;
+import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.OIConstants;
+import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.ShooterSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -17,18 +19,11 @@
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants;
-import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.OIConstants;
-import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.DriveSubsystem;
-import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.ShooterSubsystem;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
-
/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * 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.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems
@@ -38,28 +33,28 @@
// A simple autonomous routine that shoots the loaded frisbees
private final Command m_autoCommand =
// Start the command by spinning up the shooter...
- new InstantCommand(m_shooter::enable, m_shooter).andThen(
- // Wait until the shooter is at speed before feeding the frisbees
- new WaitUntilCommand(m_shooter::atSetpoint),
- // Start running the feeder
- new InstantCommand(m_shooter::runFeeder, m_shooter),
- // Shoot for the specified time
- new WaitCommand(AutoConstants.kAutoShootTimeSeconds))
+ new InstantCommand(m_shooter::enable, m_shooter)
+ .andThen(
+ // Wait until the shooter is at speed before feeding the frisbees
+ new WaitUntilCommand(m_shooter::atSetpoint),
+ // Start running the feeder
+ new InstantCommand(m_shooter::runFeeder, m_shooter),
+ // Shoot for the specified time
+ new WaitCommand(AutoConstants.kAutoShootTimeSeconds))
// Add a timeout (will end the command if, for instance, the shooter never gets up to
// speed)
.withTimeout(AutoConstants.kAutoTimeoutSeconds)
// When the command ends, turn off the shooter and the feeder
- .andThen(() -> {
- m_shooter.disable();
- m_shooter.stopFeeder();
- });
+ .andThen(
+ () -> {
+ m_shooter.disable();
+ m_shooter.stopFeeder();
+ });
// The driver's controller
XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
- /**
- * The container for the robot. Contains subsystems, OI devices, and commands.
- */
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
@@ -69,17 +64,18 @@
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
- new RunCommand(() -> m_robotDrive
- .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
- m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
-
+ new RunCommand(
+ () ->
+ m_robotDrive.arcadeDrive(
+ m_driverController.getLeftY(), m_driverController.getRightX()),
+ m_robotDrive));
}
/**
- * Use this method to define your button->command mappings. Buttons can be created by
- * instantiating a {@link GenericHID} or one of its subclasses ({@link
- * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
- * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+ * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Spin up the shooter when the 'A' button is pressed
@@ -91,22 +87,24 @@
.whenPressed(new InstantCommand(m_shooter::disable, m_shooter));
// Run the feeder when the 'X' button is held, but only if the shooter is at speed
- new JoystickButton(m_driverController, Button.kX.value).whenPressed(new ConditionalCommand(
- // Run the feeder
- new InstantCommand(m_shooter::runFeeder, m_shooter),
- // Do nothing
- new InstantCommand(),
- // Determine which of the above to do based on whether the shooter has reached the
- // desired speed
- m_shooter::atSetpoint)).whenReleased(new InstantCommand(m_shooter::stopFeeder, m_shooter));
+ new JoystickButton(m_driverController, Button.kX.value)
+ .whenPressed(
+ new ConditionalCommand(
+ // Run the feeder
+ new InstantCommand(m_shooter::runFeeder, m_shooter),
+ // Do nothing
+ new InstantCommand(),
+ // Determine which of the above to do based on whether the shooter has reached the
+ // desired speed
+ m_shooter::atSetpoint))
+ .whenReleased(new InstantCommand(m_shooter::stopFeeder, m_shooter));
// Drive at half speed when the bumper is held
- new JoystickButton(m_driverController, Button.kBumperRight.value)
+ new JoystickButton(m_driverController, Button.kRightBumper.value)
.whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
}
-
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
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 b82cb75..5773da1 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
@@ -1,48 +1,53 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.frisbeebot.subsystems;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
import 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 SpeedControllerGroup m_leftMotors =
- new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
- new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
+ private final MotorControllerGroup m_leftMotors =
+ new MotorControllerGroup(
+ new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+ new PWMSparkMax(DriveConstants.kLeftMotor2Port));
// The motors on the right side of the drive.
- private final SpeedControllerGroup m_rightMotors =
- new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
- new PWMVictorSPX(DriveConstants.kRightMotor2Port));
+ private final MotorControllerGroup m_rightMotors =
+ new MotorControllerGroup(
+ new PWMSparkMax(DriveConstants.kRightMotor1Port),
+ new PWMSparkMax(DriveConstants.kRightMotor2Port));
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
// The left-side drive encoder
private final Encoder m_leftEncoder =
- new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
- DriveConstants.kLeftEncoderReversed);
+ 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);
+ new Encoder(
+ DriveConstants.kRightEncoderPorts[0],
+ DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
- /**
- * Creates a new DriveSubsystem.
- */
+ /** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ // 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);
+
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -58,9 +63,7 @@
m_drive.arcadeDrive(fwd, rot);
}
- /**
- * Resets the drive encoders to currently read a position of 0.
- */
+ /** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
@@ -94,7 +97,7 @@
}
/**
- * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
index 8b0f3ca..f6f85f7 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
@@ -1,33 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.frisbeebot.subsystems;
+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.PWMVictorSPX;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;
-import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants;
-
public class ShooterSubsystem extends PIDSubsystem {
- private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(ShooterConstants.kShooterMotorPort);
- private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(ShooterConstants.kFeederMotorPort);
+ private final PWMSparkMax m_shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort);
+ private final PWMSparkMax m_feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort);
private final Encoder m_shooterEncoder =
- new Encoder(ShooterConstants.kEncoderPorts[0], ShooterConstants.kEncoderPorts[1],
- ShooterConstants.kEncoderReversed);
+ new Encoder(
+ ShooterConstants.kEncoderPorts[0],
+ ShooterConstants.kEncoderPorts[1],
+ ShooterConstants.kEncoderReversed);
private final SimpleMotorFeedforward m_shooterFeedforward =
- new SimpleMotorFeedforward(ShooterConstants.kSVolts,
- ShooterConstants.kVVoltSecondsPerRotation);
+ new SimpleMotorFeedforward(
+ ShooterConstants.kSVolts, ShooterConstants.kVVoltSecondsPerRotation);
- /**
- * The shooter subsystem for the robot.
- */
+ /** The shooter subsystem for the robot. */
public ShooterSubsystem() {
super(new PIDController(ShooterConstants.kP, ShooterConstants.kI, ShooterConstants.kD));
getController().setTolerance(ShooterConstants.kShooterToleranceRPS);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java
index 60aea50..8bf98fd 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot;
@@ -15,8 +12,7 @@
* call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java
index ea26e28..12fddb3 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot;
@@ -37,8 +34,8 @@
* 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.
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
@@ -49,20 +46,14 @@
CommandScheduler.getInstance().run();
}
- /**
- * This function is called once each time the robot enters Disabled mode.
- */
+ /** This function is called once each time the robot enters Disabled mode. */
@Override
- public void disabledInit() {
- }
+ public void disabledInit() {}
@Override
- public void disabledPeriodic() {
- }
+ public void disabledPeriodic() {}
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -73,12 +64,9 @@
}
}
- /**
- * This function is called periodically during autonomous.
- */
+ /** This function is called periodically during autonomous. */
@Override
- public void autonomousPeriodic() {
- }
+ public void autonomousPeriodic() {}
@Override
public void teleopInit() {
@@ -91,12 +79,9 @@
}
}
- /**
- * This function is called periodically during operator control.
- */
+ /** This function is called periodically during operator control. */
@Override
- public void teleopPeriodic() {
- }
+ public void teleopPeriodic() {}
@Override
public void testInit() {
@@ -104,10 +89,7 @@
CommandScheduler.getInstance().cancelAll();
}
- /**
- * This function is called periodically during test mode.
- */
+ /** This function is called periodically during test mode. */
@Override
- public void testPeriodic() {
- }
+ public void testPeriodic() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java
index 40d3f11..e4c7412 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java
@@ -1,21 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot;
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.GenericHID.Hand;
-import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
import edu.wpi.first.wpilibj.examples.gearsbot.commands.Autonomous;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.CloseClaw;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.OpenClaw;
@@ -26,36 +15,37 @@
import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetWristSetpoint;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.TankDrive;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
-import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * 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.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems and commands are defined here...
- private final DriveTrain m_drivetrain = new DriveTrain();
+ private final Drivetrain m_drivetrain = new Drivetrain();
private final Elevator m_elevator = new Elevator();
private final Wrist m_wrist = new Wrist();
private final Claw m_claw = new Claw();
- private final Joystick m_joystick = new Joystick(0);
+ private final XboxController m_joystick = new XboxController(0);
private final CommandBase m_autonomousCommand =
new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator);
- /**
- * The container for the robot. Contains subsystems, OI devices, and commands.
- */
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Put Some buttons on the SmartDashboard
SmartDashboard.putData("Elevator Bottom", new SetElevatorSetpoint(0, m_elevator));
- SmartDashboard.putData("Elevator Platform", new SetElevatorSetpoint(0.2, m_elevator));
- SmartDashboard.putData("Elevator Top", new SetElevatorSetpoint(0.3, m_elevator));
+ SmartDashboard.putData("Elevator Top", new SetElevatorSetpoint(0.25, m_elevator));
SmartDashboard.putData("Wrist Horizontal", new SetWristSetpoint(0, m_wrist));
SmartDashboard.putData("Raise Wrist", new SetWristSetpoint(-45, m_wrist));
@@ -63,12 +53,12 @@
SmartDashboard.putData("Open Claw", new OpenClaw(m_claw));
SmartDashboard.putData("Close Claw", new CloseClaw(m_claw));
- SmartDashboard
- .putData("Deliver Soda", new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator));
+ SmartDashboard.putData(
+ "Deliver Soda", new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator));
// Assign default commands
- m_drivetrain.setDefaultCommand(new TankDrive(() -> m_joystick.getY(Hand.kLeft),
- () -> m_joystick.getY(Hand.kRight), m_drivetrain));
+ m_drivetrain.setDefaultCommand(
+ new TankDrive(m_joystick::getLeftY, m_joystick::getRightY, m_drivetrain));
// Show what command your subsystem is running on the SmartDashboard
SmartDashboard.putData(m_drivetrain);
@@ -81,10 +71,10 @@
}
/**
- * Use this method to define your button->command mappings. Buttons can be created by
- * instantiating a {@link GenericHID} or one of its subclasses ({@link
- * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
- * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+ * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Create some buttons
@@ -98,8 +88,8 @@
final JoystickButton r1 = new JoystickButton(m_joystick, 12);
// Connect the buttons to commands
- dpadUp.whenPressed(new SetElevatorSetpoint(0.2, m_elevator));
- dpadDown.whenPressed(new SetElevatorSetpoint(-0.2, m_elevator));
+ dpadUp.whenPressed(new SetElevatorSetpoint(0.25, m_elevator));
+ dpadDown.whenPressed(new SetElevatorSetpoint(0.0, m_elevator));
dpadRight.whenPressed(new CloseClaw(m_claw));
dpadLeft.whenPressed(new OpenClaw(m_claw));
@@ -109,7 +99,6 @@
l2.whenPressed(new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator));
}
-
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java
index 0f0db70..599ed69 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java
@@ -1,27 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot.commands;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
-import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-/**
- * The main autonomous command to pickup and deliver the soda to the box.
- */
+/** The main autonomous command to pickup and deliver the soda to the box. */
public class Autonomous extends SequentialCommandGroup {
- /**
- * Create a new autonomous command.
- */
- public Autonomous(DriveTrain drive, Claw claw, Wrist wrist, Elevator elevator) {
+ /** Create a new autonomous command. */
+ public Autonomous(Drivetrain drive, Claw claw, Wrist wrist, Elevator elevator) {
addCommands(
new PrepareToPickup(claw, wrist, elevator),
new Pickup(claw, wrist, elevator),
@@ -30,10 +22,6 @@
new Place(claw, wrist, elevator),
new SetDistanceToBox(0.60, drive),
// new DriveStraight(-2), // Use Encoders if ultrasonic is broken
- parallel(
- new SetWristSetpoint(-45, wrist),
- new CloseClaw(claw)
- )
- );
+ parallel(new SetWristSetpoint(-45, wrist), new CloseClaw(claw)));
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java
index 0ebce9a..8ef1deb 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java
@@ -1,20 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot.commands;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
+import edu.wpi.first.wpilibj2.command.CommandBase;
-/**
- * Closes the claw for one second. Real robots should use sensors, stalling motors is BAD!
- */
+/** Closes the claw until the limit switch is tripped. */
public class CloseClaw extends CommandBase {
private final Claw m_claw;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java
index afed30d..ed47cc1 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java
@@ -1,35 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot.commands;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
import edu.wpi.first.wpilibj2.command.PIDCommand;
-import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
-
/**
- * Drive the given distance straight (negative values go backwards). Uses a
- * local PID controller to run a simple PID loop that is only enabled while this
- * command is running. The input is the averaged values of the left and right
- * encoders.
+ * Drive the given distance straight (negative values go backwards). Uses a local PID controller to
+ * run a simple PID loop that is only enabled while this command is running. The input is the
+ * averaged values of the left and right encoders.
*/
public class DriveStraight extends PIDCommand {
- private final DriveTrain m_drivetrain;
+ private final Drivetrain m_drivetrain;
/**
* Create a new DriveStraight command.
+ *
* @param distance The distance to drive
*/
- public DriveStraight(double distance, DriveTrain drivetrain) {
- super(new PIDController(4, 0, 0),
- drivetrain::getDistance,
- distance,
- d -> drivetrain.drive(d, d));
+ public DriveStraight(double distance, Drivetrain drivetrain) {
+ super(
+ new PIDController(4, 0, 0), drivetrain::getDistance, distance, d -> drivetrain.drive(d, d));
m_drivetrain = drivetrain;
addRequirements(m_drivetrain);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java
index 9d944b4..cf1754e 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java
@@ -1,19 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot.commands;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
import edu.wpi.first.wpilibj2.command.WaitCommand;
-import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
-
-/**
- * Opens the claw for one second. Real robots should use sensors, stalling motors is BAD!
- */
+/** Opens the claw for one second. Real robots should use sensors, stalling motors is BAD! */
public class OpenClaw extends WaitCommand {
private final Claw m_claw;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java
index b51d106..4c7d267 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java
@@ -1,18 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot.commands;
-
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
/**
* Pickup a soda can (if one is between the open claws) and get it in a safe state to drive around.
@@ -21,15 +16,13 @@
/**
* Create a new pickup command.
*
- * @param claw The claw subsystem to use
- * @param wrist The wrist subsystem to use
+ * @param claw The claw subsystem to use
+ * @param wrist The wrist subsystem to use
* @param elevator The elevator subsystem to use
*/
public Pickup(Claw claw, Wrist wrist, Elevator elevator) {
addCommands(
new CloseClaw(claw),
- parallel(
- new SetWristSetpoint(-45, wrist),
- new SetElevatorSetpoint(0.25, elevator)));
+ parallel(new SetWristSetpoint(-45, wrist), new SetElevatorSetpoint(0.25, elevator)));
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java
index c57ffe2..442744c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java
@@ -1,28 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot.commands;
-
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-/**
- * Place a held soda can onto the platform.
- */
+/** Place a held soda can onto the platform. */
public class Place extends SequentialCommandGroup {
/**
* Create a new place command.
*
- * @param claw The claw subsystem to use
- * @param wrist The wrist subsystem to use
+ * @param claw The claw subsystem to use
+ * @param wrist The wrist subsystem to use
* @param elevator The elevator subsystem to use
*/
public Place(Claw claw, Wrist wrist, Elevator elevator) {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java
index 620c184..1ae5992 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java
@@ -1,34 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot.commands;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-/**
- * Make sure the robot is in a state to pickup soda cans.
- */
+/** Make sure the robot is in a state to pickup soda cans. */
public class PrepareToPickup extends SequentialCommandGroup {
/**
* Create a new prepare to pickup command.
*
- * @param claw The claw subsystem to use
- * @param wrist The wrist subsystem to use
+ * @param claw The claw subsystem to use
+ * @param wrist The wrist subsystem to use
* @param elevator The elevator subsystem to use
*/
public PrepareToPickup(Claw claw, Wrist wrist, Elevator elevator) {
addCommands(
new OpenClaw(claw),
- parallel(
- new SetWristSetpoint(0, wrist),
- new SetElevatorSetpoint(0, elevator)));
+ parallel(new SetWristSetpoint(0, wrist), new SetElevatorSetpoint(0, elevator)));
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java
index b14ddc9..f8b291c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java
@@ -1,35 +1,31 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot.commands;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
import edu.wpi.first.wpilibj2.command.PIDCommand;
-import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
-
-
/**
- * Drive until the robot is the given distance away from the box. Uses a local
- * PID controller to run a simple PID loop that is only enabled while this
- * command is running. The input is the averaged values of the left and right
- * encoders.
+ * Drive until the robot is the given distance away from the box. Uses a local PID controller to run
+ * a simple PID loop that is only enabled while this command is running. The input is the averaged
+ * values of the left and right encoders.
*/
public class SetDistanceToBox extends PIDCommand {
- private final DriveTrain m_drivetrain;
+ private final Drivetrain m_drivetrain;
/**
* Create a new set distance to box command.
*
* @param distance The distance away from the box to drive to
*/
- public SetDistanceToBox(double distance, DriveTrain drivetrain) {
- super(new PIDController(-2, 0, 0),
- drivetrain::getDistanceToObstacle, distance,
+ public SetDistanceToBox(double distance, Drivetrain drivetrain) {
+ super(
+ new PIDController(-2, 0, 0),
+ drivetrain::getDistanceToObstacle,
+ distance,
d -> drivetrain.drive(d, d));
m_drivetrain = drivetrain;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java
index 44c6677..a323305 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java
@@ -1,16 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot.commands;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
-
+import edu.wpi.first.wpilibj2.command.CommandBase;
/**
* Move the elevator to a given location. This command finishes when it is within the tolerance, but
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java
index d03211a..4c45ddf 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java
@@ -1,16 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot.commands;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
-
+import edu.wpi.first.wpilibj2.command.CommandBase;
/**
* Move the wrist to a given angle. This command finishes when it is within the tolerance, but
@@ -25,7 +20,7 @@
* Create a new SetWristSetpoint command.
*
* @param setpoint The setpoint to set the wrist to
- * @param wrist The wrist to use
+ * @param wrist The wrist to use
*/
public SetWristSetpoint(double setpoint, Wrist wrist) {
m_wrist = wrist;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDrive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDrive.java
index b5c24f2..009daad 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDrive.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDrive.java
@@ -1,35 +1,27 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot.commands;
-
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
import java.util.function.DoubleSupplier;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
-
-/**
- * Have the robot drive tank style.
- */
+/** Have the robot drive tank style. */
public class TankDrive extends CommandBase {
- private final DriveTrain m_drivetrain;
+ private final Drivetrain m_drivetrain;
private final DoubleSupplier m_left;
private final DoubleSupplier m_right;
/**
* Creates a new TankDrive command.
*
- * @param left The control input for the left side of the drive
- * @param right The control input for the right sight of the drive
+ * @param left The control input for the left side of the drive
+ * @param right The control input for the right sight of the drive
* @param drivetrain The drivetrain subsystem to drive
*/
- public TankDrive(DoubleSupplier left, DoubleSupplier right, DriveTrain drivetrain) {
+ public TankDrive(DoubleSupplier left, DoubleSupplier right, Drivetrain drivetrain) {
m_drivetrain = drivetrain;
m_left = left;
m_right = right;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java
index 86859f1..d34d12a 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot.subsystems;
import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.motorcontrol.Victor;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -20,9 +17,7 @@
private final Victor m_motor = new Victor(7);
private final DigitalInput m_contact = new DigitalInput(5);
- /**
- * Create a new claw subsystem.
- */
+ /** Create a new claw subsystem. */
public Claw() {
// Let's name everything on the LiveWindow
addChild("Motor", m_motor);
@@ -33,37 +28,27 @@
SmartDashboard.putData("Claw switch", m_contact);
}
- /**
- * Set the claw motor to move in the open direction.
- */
+ /** Set the claw motor to move in the open direction. */
public void open() {
m_motor.set(-1);
}
- /**
- * Set the claw motor to move in the close direction.
- */
+ /** Set the claw motor to move in the close direction. */
public void close() {
m_motor.set(1);
}
- /**
- * Stops the claw motor from moving.
- */
+ /** Stops the claw motor from moving. */
public void stop() {
m_motor.set(0);
}
- /**
- * Return true when the robot is grabbing an object hard enough to trigger the limit switch.
- */
+ /** Return true when the robot is grabbing an object hard enough to trigger the limit switch. */
public boolean isGrabbing() {
return m_contact.get();
}
- /**
- * Call log method every loop.
- */
+ /** Call log method every loop. */
@Override
public void periodic() {
log();
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
similarity index 69%
rename from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java
rename to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java
index 9c85429..44f455d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java
@@ -1,33 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot.subsystems;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+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;
-import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
-
-public class DriveTrain extends SubsystemBase {
+public class Drivetrain extends SubsystemBase {
/**
- * The DriveTrain subsystem incorporates the sensors and actuators attached to the robots chassis.
+ * The Drivetrain subsystem incorporates the sensors and actuators attached to the robots chassis.
* These include four drive motors, a left and right encoder and a gyro.
*/
- private final SpeedController m_leftMotor =
- new SpeedControllerGroup(new PWMVictorSPX(0), new PWMVictorSPX(1));
- private final SpeedController m_rightMotor =
- new SpeedControllerGroup(new PWMVictorSPX(2), new PWMVictorSPX(3));
+ private final MotorController m_leftMotor =
+ new MotorControllerGroup(new PWMSparkMax(0), new PWMSparkMax(1));
+
+ private final MotorController m_rightMotor =
+ new MotorControllerGroup(new PWMSparkMax(2), new PWMSparkMax(3));
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotor, m_rightMotor);
@@ -36,12 +33,15 @@
private final AnalogInput m_rangefinder = new AnalogInput(6);
private final AnalogGyro m_gyro = new AnalogGyro(1);
- /**
- * Create a new drive train subsystem.
- */
- public DriveTrain() {
+ /** Create a new drivetrain subsystem. */
+ public Drivetrain() {
super();
+ // 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);
+
// Encoders may measure differently in the real world and in
// simulation. In this example the robot moves 0.042 barleycorns
// per tick in the real world, but the simulated encoders
@@ -51,7 +51,7 @@
m_leftEncoder.setDistancePerPulse(0.042);
m_rightEncoder.setDistancePerPulse(0.042);
} else {
- // Circumference in ft = 4in/12(in/ft)*PI
+ // Circumference = diameter in feet * pi. 360 tick simulated encoders.
m_leftEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
m_rightEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
}
@@ -64,9 +64,7 @@
addChild("Gyro", m_gyro);
}
- /**
- * The log method puts interesting information to the SmartDashboard.
- */
+ /** The log method puts interesting information to the SmartDashboard. */
public void log() {
SmartDashboard.putNumber("Left Distance", m_leftEncoder.getDistance());
SmartDashboard.putNumber("Right Distance", m_rightEncoder.getDistance());
@@ -76,9 +74,9 @@
}
/**
- * Tank style driving for the DriveTrain.
+ * Tank style driving for the Drivetrain.
*
- * @param left Speed in range [-1,1]
+ * @param left Speed in range [-1,1]
* @param right Speed in range [-1,1]
*/
public void drive(double left, double right) {
@@ -94,9 +92,7 @@
return m_gyro.getAngle();
}
- /**
- * Reset the robots sensors to the zero states.
- */
+ /** Reset the robots sensors to the zero states. */
public void reset() {
m_gyro.reset();
m_leftEncoder.reset();
@@ -122,9 +118,7 @@
return m_rangefinder.getAverageVoltage();
}
- /**
- * Call log method every loop.
- */
+ /** Call log method every loop. */
@Override
public void periodic() {
log();
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
index 5492046..e174595 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
@@ -1,20 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot.subsystems;
+import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
-import edu.wpi.first.wpilibj.Victor;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+import edu.wpi.first.wpilibj.motorcontrol.Victor;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;
-import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
-
/**
* The elevator subsystem uses PID to go to a given height. Unfortunately, in it's current state PID
* values for simulation are different than in the real world do to minor differences.
@@ -28,9 +24,7 @@
private static final double kP_simulation = 18;
private static final double kI_simulation = 0.2;
- /**
- * Create a new elevator subsystem.
- */
+ /** Create a new elevator subsystem. */
public Elevator() {
super(new PIDController(kP_real, kI_real, 0));
if (Robot.isSimulation()) { // Check for simulation and update PID values
@@ -53,9 +47,7 @@
addChild("Pot", m_pot);
}
- /**
- * The log method puts interesting information to the SmartDashboard.
- */
+ /** The log method puts interesting information to the SmartDashboard. */
public void log() {
SmartDashboard.putData("Elevator Pot", m_pot);
}
@@ -68,17 +60,13 @@
return m_pot.get();
}
- /**
- * Use the motor as the PID output. This method is automatically called by the subsystem.
- */
+ /** Use the motor as the PID output. This method is automatically called by the subsystem. */
@Override
public void useOutput(double output, double setpoint) {
m_motor.set(output);
}
- /**
- * Call log method every loop.
- */
+ /** Call log method every loop. */
@Override
public void periodic() {
log();
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
index 1b32139..f176316 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
@@ -1,20 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gearsbot.subsystems;
+import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
-import edu.wpi.first.wpilibj.Victor;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+import edu.wpi.first.wpilibj.motorcontrol.Victor;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;
-import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
-
/**
* The wrist subsystem is like the elevator, but with a rotational joint instead of a linear joint.
*/
@@ -25,9 +21,7 @@
private static final double kP_real = 1;
private static final double kP_simulation = 0.05;
- /**
- * Create a new wrist subsystem.
- */
+ /** Create a new wrist subsystem. */
public Wrist() {
super(new PIDController(kP_real, 0, 0));
if (Robot.isSimulation()) { // Check for simulation and update PID values
@@ -50,9 +44,7 @@
addChild("Pot", m_pot);
}
- /**
- * The log method puts interesting information to the SmartDashboard.
- */
+ /** The log method puts interesting information to the SmartDashboard. */
public void log() {
SmartDashboard.putData("Wrist Angle", m_pot);
}
@@ -65,17 +57,13 @@
return m_pot.get();
}
- /**
- * Use the motor as the PID output. This method is automatically called by the subsystem.
- */
+ /** Use the motor as the PID output. This method is automatically called by the subsystem. */
@Override
public void useOutput(double output, double setpoint) {
m_motor.set(output);
}
- /**
- * Call log method every loop.
- */
+ /** Call log method every loop. */
@Override
public void periodic() {
log();
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Main.java
index 282f922..dd467ee 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gettingstarted;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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 8b2f249..0a9a2b0 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
@@ -1,51 +1,42 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gettingstarted;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the TimedRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the manifest file in the resource
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the manifest file in the resource
* directory.
*/
public class Robot extends TimedRobot {
- private final DifferentialDrive m_robotDrive
- = new DifferentialDrive(new PWMVictorSPX(0), new PWMVictorSPX(1));
+ private final DifferentialDrive m_robotDrive =
+ new DifferentialDrive(new PWMSparkMax(0), new PWMSparkMax(1));
private final Joystick m_stick = new Joystick(0);
private final Timer m_timer = new Timer();
/**
- * This function is run when the robot is first started up and should be
- * used for any initialization code.
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
*/
@Override
- public void robotInit() {
- }
+ public void robotInit() {}
- /**
- * This function is run once each time the robot enters autonomous mode.
- */
+ /** This function is run once each time the robot enters autonomous mode. */
@Override
public void autonomousInit() {
m_timer.reset();
m_timer.start();
}
- /**
- * This function is called periodically during autonomous.
- */
+ /** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {
// Drive for 2 seconds
@@ -56,25 +47,21 @@
}
}
- /**
- * This function is called once each time the robot enters teleoperated mode.
- */
+ /** This function is called once each time the robot enters teleoperated mode. */
@Override
- public void teleopInit() {
- }
+ public void teleopInit() {}
- /**
- * This function is called periodically during teleoperated mode.
- */
+ /** This function is called periodically during teleoperated mode. */
@Override
public void teleopPeriodic() {
m_robotDrive.arcadeDrive(m_stick.getY(), m_stick.getX());
}
- /**
- * This function is called periodically during test mode.
- */
+ /** This function is called once each time the robot enters test mode. */
@Override
- public void testPeriodic() {
- }
+ public void testInit() {}
+
+ /** This function is called periodically during test mode. */
+ @Override
+ public void testPeriodic() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Main.java
index 4c5574e..e8f2df6 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gyro;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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 39b2ecb..3d8353b 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
@@ -1,22 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gyro;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
- * This is a sample program to demonstrate how to use a gyro sensor to make a
- * robot drive straight. This program uses a joystick to drive forwards and
- * backwards while the gyro is used for direction keeping.
+ * This is a sample program to demonstrate how to use a gyro sensor to make a robot drive straight.
+ * This program uses a joystick to drive forwards and backwards while the gyro is used for direction
+ * keeping.
*/
public class Robot extends TimedRobot {
private static final double kAngleSetpoint = 0.0;
@@ -31,9 +28,8 @@
private static final int kGyroPort = 0;
private static final int kJoystickPort = 0;
- private final DifferentialDrive m_myRobot
- = new DifferentialDrive(new PWMVictorSPX(kLeftMotorPort),
- new PWMVictorSPX(kRightMotorPort));
+ private final DifferentialDrive m_myRobot =
+ new DifferentialDrive(new PWMSparkMax(kLeftMotorPort), new PWMSparkMax(kRightMotorPort));
private final AnalogGyro m_gyro = new AnalogGyro(kGyroPort);
private final Joystick m_joystick = new Joystick(kJoystickPort);
@@ -43,8 +39,8 @@
}
/**
- * The motor speed is set from the joystick while the RobotDrive turning
- * value is assigned from the error between the setpoint and the gyro angle.
+ * The motor speed is set from the joystick while the DifferentialDrive turning value is assigned
+ * from the error between the setpoint and the gyro angle.
*/
@Override
public void teleopPeriodic() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java
index 473d6e1..b59c23d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gyrodrivecommands;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants. This class should not be used for any other purpose. All constants should be
- * declared globally (i.e. public static). Do not put anything functional in this class.
+ * 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.
@@ -22,8 +19,8 @@
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
- public static final int[] kLeftEncoderPorts = new int[]{0, 1};
- public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+ public static final int[] kRightEncoderPorts = new int[] {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
@@ -51,6 +48,6 @@
}
public static final class OIConstants {
- public static final int kDriverControllerPort = 1;
+ public static final int kDriverControllerPort = 0;
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java
index f18e95c..9e6bfe7 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gyrodrivecommands;
@@ -15,8 +12,7 @@
* call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java
index 40c6db9..fff4d8c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gyrodrivecommands;
@@ -37,8 +34,8 @@
* 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.
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
@@ -49,20 +46,14 @@
CommandScheduler.getInstance().run();
}
- /**
- * This function is called once each time the robot enters Disabled mode.
- */
+ /** This function is called once each time the robot enters Disabled mode. */
@Override
- public void disabledInit() {
- }
+ public void disabledInit() {}
@Override
- public void disabledPeriodic() {
- }
+ public void disabledPeriodic() {}
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
}
}
- /**
- * This function is called periodically during autonomous.
- */
+ /** This function is called periodically during autonomous. */
@Override
- public void autonomousPeriodic() {
- }
+ public void autonomousPeriodic() {}
@Override
public void teleopInit() {
@@ -98,13 +86,9 @@
}
}
- /**
- * This function is called periodically during operator control.
- */
+ /** This function is called periodically during operator control. */
@Override
- public void teleopPeriodic() {
-
- }
+ public void teleopPeriodic() {}
@Override
public void testInit() {
@@ -112,10 +96,7 @@
CommandScheduler.getInstance().cancelAll();
}
- /**
- * This function is called periodically during test mode.
- */
+ /** This function is called periodically during test mode. */
@Override
- public void testPeriodic() {
- }
+ public void testPeriodic() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
index 1b94fea..1356a37 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
@@ -1,45 +1,38 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gyrodrivecommands;
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import static edu.wpi.first.wpilibj.PS4Controller.Button;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.PS4Controller;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.OIConstants;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngle;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngleProfiled;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.PIDCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.OIConstants;
-import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngle;
-import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngleProfiled;
-import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
-
/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * 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.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
// The driver's controller
- XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
+ PS4Controller m_driverController = new PS4Controller(OIConstants.kDriverControllerPort);
- /**
- * The container for the robot. Contains subsystems, OI devices, and commands.
- */
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
@@ -49,47 +42,51 @@
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
- new RunCommand(() -> m_robotDrive
- .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
- m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
-
+ new RunCommand(
+ () ->
+ m_robotDrive.arcadeDrive(
+ m_driverController.getLeftY(), m_driverController.getRightX()),
+ m_robotDrive));
}
/**
- * Use this method to define your button->command mappings. Buttons can be created by
- * instantiating a {@link GenericHID} or one of its subclasses ({@link
- * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
- * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link PS4Controller}), and then passing it to a {@link
+ * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Drive at half speed when the right bumper is held
- new JoystickButton(m_driverController, Button.kBumperRight.value)
+ new JoystickButton(m_driverController, Button.kR1.value)
.whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
// Stabilize robot to drive straight with gyro when left bumper is held
- new JoystickButton(m_driverController, Button.kBumperLeft.value).whenHeld(new PIDCommand(
- new PIDController(DriveConstants.kStabilizationP, DriveConstants.kStabilizationI,
- DriveConstants.kStabilizationD),
- // Close the loop on the turn rate
- m_robotDrive::getTurnRate,
- // Setpoint is 0
- 0,
- // Pipe the output to the turning controls
- output -> m_robotDrive.arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), output),
- // Require the robot drive
- m_robotDrive));
+ new JoystickButton(m_driverController, Button.kL1.value)
+ .whenHeld(
+ new PIDCommand(
+ new PIDController(
+ DriveConstants.kStabilizationP,
+ DriveConstants.kStabilizationI,
+ DriveConstants.kStabilizationD),
+ // Close the loop on the turn rate
+ m_robotDrive::getTurnRate,
+ // Setpoint is 0
+ 0,
+ // Pipe the output to the turning controls
+ output -> m_robotDrive.arcadeDrive(m_driverController.getLeftY(), output),
+ // Require the robot drive
+ m_robotDrive));
// Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout
- new JoystickButton(m_driverController, Button.kX.value)
+ new JoystickButton(m_driverController, Button.kCross.value)
.whenPressed(new TurnToAngle(90, m_robotDrive).withTimeout(5));
- // Turn to -90 degrees with a profile when the 'A' button is pressed, with a 5 second timeout
- new JoystickButton(m_driverController, Button.kA.value)
+ // Turn to -90 degrees with a profile when the Circle button is pressed, with a 5 second timeout
+ new JoystickButton(m_driverController, Button.kCircle.value)
.whenPressed(new TurnToAngleProfiled(-90, m_robotDrive).withTimeout(5));
}
-
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java
index 6c8ea36..053c644 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java
@@ -1,27 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gyrodrivecommands.commands;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj2.command.PIDCommand;
-
+import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
-/**
- * A command that will turn the robot to the specified angle.
- */
+/** A command that will turn the robot to the specified angle. */
public class TurnToAngle extends PIDCommand {
/**
* Turns to robot to the specified angle.
*
* @param targetAngleDegrees The angle to turn to
- * @param drive The drive subsystem to use
+ * @param drive The drive subsystem to use
*/
public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) {
super(
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java
index 44f24d3..1481fab 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java
@@ -1,35 +1,32 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gyrodrivecommands.commands;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
-
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
-/**
- * A command that will turn the robot to the specified angle using a motion profile.
- */
+/** A command that will turn the robot to the specified angle using a motion profile. */
public class TurnToAngleProfiled extends ProfiledPIDCommand {
/**
* Turns to robot to the specified angle using a motion profile.
*
* @param targetAngleDegrees The angle to turn to
- * @param drive The drive subsystem to use
+ * @param drive The drive subsystem to use
*/
public TurnToAngleProfiled(double targetAngleDegrees, DriveSubsystem drive) {
super(
- new ProfiledPIDController(DriveConstants.kTurnP, DriveConstants.kTurnI,
- DriveConstants.kTurnD, new TrapezoidProfile.Constraints(
- DriveConstants.kMaxTurnRateDegPerS,
- DriveConstants.kMaxTurnAccelerationDegPerSSquared)),
+ new ProfiledPIDController(
+ DriveConstants.kTurnP,
+ DriveConstants.kTurnI,
+ DriveConstants.kTurnD,
+ new TrapezoidProfile.Constraints(
+ DriveConstants.kMaxTurnRateDegPerS,
+ DriveConstants.kMaxTurnAccelerationDegPerSSquared)),
// Close loop on heading
drive::getHeading,
// Set reference to target
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 7bdeb24..a2fdf6e 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
@@ -1,53 +1,58 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gyrodrivecommands.subsystems;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+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 SpeedControllerGroup m_leftMotors =
- new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
- new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
+ private final MotorControllerGroup m_leftMotors =
+ new MotorControllerGroup(
+ new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+ new PWMSparkMax(DriveConstants.kLeftMotor2Port));
// The motors on the right side of the drive.
- private final SpeedControllerGroup m_rightMotors =
- new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
- new PWMVictorSPX(DriveConstants.kRightMotor2Port));
+ private final MotorControllerGroup m_rightMotors =
+ new MotorControllerGroup(
+ new PWMSparkMax(DriveConstants.kRightMotor1Port),
+ new PWMSparkMax(DriveConstants.kRightMotor2Port));
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
// The left-side drive encoder
private final Encoder m_leftEncoder =
- new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
- DriveConstants.kLeftEncoderReversed);
+ 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);
+ new Encoder(
+ DriveConstants.kRightEncoderPorts[0],
+ DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
// The gyro sensor
private final Gyro m_gyro = new ADXRS450_Gyro();
- /**
- * Creates a new DriveSubsystem.
- */
+ /** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ // 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);
+
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -63,9 +68,7 @@
m_drive.arcadeDrive(fwd, rot);
}
- /**
- * Resets the drive encoders to currently read a position of 0.
- */
+ /** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
@@ -99,7 +102,7 @@
}
/**
- * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
@@ -107,9 +110,7 @@
m_drive.setMaxOutput(maxOutput);
}
- /**
- * Zeroes the heading of the robot.
- */
+ /** Zeroes the heading of the robot. */
public void zeroHeading() {
m_gyro.reset();
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Main.java
index 63300b6..8c952f1 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gyromecanum;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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 622fda4..f4b9a97 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
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.gyromecanum;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
- * This is a sample program that uses mecanum drive with a gyro sensor to
- * maintain rotation vectorsin relation to the starting orientation of the robot
- * (field-oriented controls).
+ * This is a sample program that uses mecanum drive with a gyro sensor to maintain rotation
+ * vectorsin relation to the starting orientation of the robot (field-oriented controls).
*/
public class Robot extends TimedRobot {
// gyro calibration constant, may need to be adjusted;
@@ -36,10 +32,10 @@
@Override
public void robotInit() {
- PWMVictorSPX frontLeft = new PWMVictorSPX(kFrontLeftChannel);
- PWMVictorSPX rearLeft = new PWMVictorSPX(kRearLeftChannel);
- PWMVictorSPX frontRight = new PWMVictorSPX(kFrontRightChannel);
- PWMVictorSPX rearRight = new PWMVictorSPX(kRearRightChannel);
+ PWMSparkMax frontLeft = new PWMSparkMax(kFrontLeftChannel);
+ PWMSparkMax rearLeft = new PWMSparkMax(kRearLeftChannel);
+ PWMSparkMax frontRight = new PWMSparkMax(kFrontRightChannel);
+ PWMSparkMax rearRight = new PWMSparkMax(kRearRightChannel);
// Invert the left side motors.
// You may need to change or remove this to match your robot.
@@ -51,12 +47,10 @@
m_gyro.setSensitivity(kVoltsPerDegreePerSecond);
}
- /**
- * Mecanum drive is used with the gyro angle as an input.
- */
+ /** Mecanum drive is used with the gyro angle as an input. */
@Override
public void teleopPeriodic() {
- m_robotDrive.driveCartesian(m_joystick.getX(), m_joystick.getY(),
- m_joystick.getZ(), m_gyro.getAngle());
+ m_robotDrive.driveCartesian(
+ m_joystick.getX(), m_joystick.getY(), m_joystick.getZ(), m_gyro.getAngle());
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java
index 8029888..30e8aad 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbotinlined;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants. This class should not be used for any other purpose. All constants should be
- * declared globally (i.e. public static). Do not put anything functional in this class.
+ * 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.
@@ -22,8 +19,8 @@
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
- public static final int[] kLeftEncoderPorts = new int[]{0, 1};
- public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+ public static final int[] kRightEncoderPorts = new int[] {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
@@ -36,7 +33,7 @@
public static final class HatchConstants {
public static final int kHatchSolenoidModule = 0;
- public static final int[] kHatchSolenoidPorts = new int[]{0, 1};
+ public static final int[] kHatchSolenoidPorts = new int[] {0, 1};
}
public static final class AutoConstants {
@@ -46,6 +43,6 @@
}
public static final class OIConstants {
- public static final int kDriverControllerPort = 1;
+ public static final int kDriverControllerPort = 0;
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Main.java
index 3852d41..114bc14 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbotinlined;
@@ -15,8 +12,7 @@
* call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Robot.java
index c0c78ac..8f16ce5 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbotinlined;
@@ -37,8 +34,8 @@
* 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.
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
@@ -49,20 +46,14 @@
CommandScheduler.getInstance().run();
}
- /**
- * This function is called once each time the robot enters Disabled mode.
- */
+ /** This function is called once each time the robot enters Disabled mode. */
@Override
- public void disabledInit() {
- }
+ public void disabledInit() {}
@Override
- public void disabledPeriodic() {
- }
+ public void disabledPeriodic() {}
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -73,12 +64,9 @@
}
}
- /**
- * This function is called periodically during autonomous.
- */
+ /** This function is called periodically during autonomous. */
@Override
- public void autonomousPeriodic() {
- }
+ public void autonomousPeriodic() {}
@Override
public void teleopInit() {
@@ -91,12 +79,9 @@
}
}
- /**
- * This function is called periodically during operator control.
- */
+ /** This function is called periodically during operator control. */
@Override
- public void teleopPeriodic() {
- }
+ public void teleopPeriodic() {}
@Override
public void testInit() {
@@ -104,10 +89,7 @@
CommandScheduler.getInstance().cancelAll();
}
- /**
- * This function is called periodically during test mode.
- */
+ /** This function is called periodically during test mode. */
@Override
- public void testPeriodic() {
- }
+ public void testPeriodic() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
index d49fba4..2207379 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
@@ -1,35 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbotinlined;
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.StartEndCommand;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import static edu.wpi.first.wpilibj.PS4Controller.Button;
+import edu.wpi.first.wpilibj.PS4Controller;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.commands.ComplexAutoCommand;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * 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.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems
@@ -39,18 +34,18 @@
// The autonomous routines
// A simple auto routine that drives forward a specified distance, and then stops.
- private final Command m_simpleAuto = new StartEndCommand(
- // Start driving forward at the start of the command
- () -> m_robotDrive.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0),
- // Stop driving at the end of the command
- () -> m_robotDrive.arcadeDrive(0, 0),
- // Requires the drive subsystem
- m_robotDrive)
- // Reset the encoders before starting
- .beforeStarting(m_robotDrive::resetEncoders)
- // End the command when the robot's driven distance exceeds the desired value
- .withInterrupt(
- () -> m_robotDrive.getAverageEncoderDistance() >= AutoConstants.kAutoDriveDistanceInches);
+ private final Command m_simpleAuto =
+ new FunctionalCommand(
+ // Reset encoders on command start
+ m_robotDrive::resetEncoders,
+ // Drive forward while the command is executing
+ () -> m_robotDrive.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0),
+ // Stop driving at the end of the command
+ interrupt -> m_robotDrive.arcadeDrive(0, 0),
+ // End the command when the robot's driven distance exceeds the desired value
+ () -> m_robotDrive.getAverageEncoderDistance() >= AutoConstants.kAutoDriveDistanceInches,
+ // Require the drive subsystem
+ m_robotDrive);
// A complex auto routine that drives forward, drops a hatch, and then drives backward.
private final Command m_complexAuto = new ComplexAutoCommand(m_robotDrive, m_hatchSubsystem);
@@ -59,11 +54,9 @@
SendableChooser<Command> m_chooser = new SendableChooser<>();
// The driver's controller
- XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
+ PS4Controller m_driverController = new PS4Controller(OIConstants.kDriverControllerPort);
- /**
- * The container for the robot. Contains subsystems, OI devices, and commands.
- */
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
@@ -73,9 +66,11 @@
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
- new RunCommand(() -> m_robotDrive
- .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
- m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
+ new RunCommand(
+ () ->
+ m_robotDrive.arcadeDrive(
+ m_driverController.getLeftY(), m_driverController.getRightX()),
+ m_robotDrive));
// Add commands to the autonomous command chooser
m_chooser.setDefaultOption("Simple Auto", m_simpleAuto);
@@ -86,25 +81,24 @@
}
/**
- * Use this method to define your button->command mappings. Buttons can be created by
- * instantiating a {@link GenericHID} or one of its subclasses ({@link
- * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
- * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link PS4Controller}), and then passing it to a {@link
+ * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
- // Grab the hatch when the 'A' button is pressed.
- new JoystickButton(m_driverController, Button.kA.value)
+ // Grab the hatch when the Circle button is pressed.
+ new JoystickButton(m_driverController, Button.kCircle.value)
.whenPressed(new InstantCommand(m_hatchSubsystem::grabHatch, m_hatchSubsystem));
- // Release the hatch when the 'B' button is pressed.
- new JoystickButton(m_driverController, Button.kB.value)
+ // Release the hatch when the Square button is pressed.
+ new JoystickButton(m_driverController, Button.kSquare.value)
.whenPressed(new InstantCommand(m_hatchSubsystem::releaseHatch, m_hatchSubsystem));
- // While holding the shoulder button, drive at half speed
- new JoystickButton(m_driverController, Button.kBumperRight.value)
+ // While holding R1, drive at half speed
+ new JoystickButton(m_driverController, Button.kR1.value)
.whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
}
-
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java
index 45674da..1faf45a 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java
@@ -1,23 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbotinlined.commands;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.StartEndCommand;
-
import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-/**
- * A complex auto command that drives forward, releases a hatch, and then drives backward.
- */
+/** A complex auto command that drives forward, releases a hatch, and then drives backward. */
public class ComplexAutoCommand extends SequentialCommandGroup {
/**
* Creates a new ComplexAutoCommand.
@@ -28,28 +22,36 @@
public ComplexAutoCommand(DriveSubsystem driveSubsystem, HatchSubsystem hatchSubsystem) {
addCommands(
// Drive forward up to the front of the cargo ship
- new StartEndCommand(
- // Start driving forward at the start of the command
+ new FunctionalCommand(
+ // Reset encoders on command start
+ driveSubsystem::resetEncoders,
+ // Drive forward while the command is executing
() -> driveSubsystem.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0),
// Stop driving at the end of the command
- () -> driveSubsystem.arcadeDrive(0, 0), driveSubsystem)
- // Reset the encoders before starting
- .beforeStarting(driveSubsystem::resetEncoders)
+ interrupt -> driveSubsystem.arcadeDrive(0, 0),
// End the command when the robot's driven distance exceeds the desired value
- .withInterrupt(() -> driveSubsystem.getAverageEncoderDistance()
- >= AutoConstants.kAutoDriveDistanceInches),
+ () ->
+ driveSubsystem.getAverageEncoderDistance()
+ >= AutoConstants.kAutoDriveDistanceInches,
+ // Require the drive subsystem
+ driveSubsystem),
// Release the hatch
new InstantCommand(hatchSubsystem::releaseHatch, hatchSubsystem),
// Drive backward the specified distance
- new StartEndCommand(
+ new FunctionalCommand(
+ // Reset encoders on command start
+ driveSubsystem::resetEncoders,
+ // Drive backward while the command is executing
() -> driveSubsystem.arcadeDrive(-AutoConstants.kAutoDriveSpeed, 0),
- () -> driveSubsystem.arcadeDrive(0, 0), driveSubsystem)
- .beforeStarting(driveSubsystem::resetEncoders)
- .withInterrupt(
- () -> driveSubsystem.getAverageEncoderDistance()
- <= -AutoConstants.kAutoBackupDistanceInches));
+ // Stop driving at the end of the command
+ interrupt -> driveSubsystem.arcadeDrive(0, 0),
+ // End the command when the robot's driven distance exceeds the desired value
+ () ->
+ driveSubsystem.getAverageEncoderDistance()
+ <= AutoConstants.kAutoBackupDistanceInches,
+ // Require the drive subsystem
+ driveSubsystem));
}
-
}
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 5d16a44..115ee02 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
@@ -1,48 +1,53 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbotinlined.subsystems;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
import 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 SpeedControllerGroup m_leftMotors =
- new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
- new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
+ private final MotorControllerGroup m_leftMotors =
+ new MotorControllerGroup(
+ new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+ new PWMSparkMax(DriveConstants.kLeftMotor2Port));
// The motors on the right side of the drive.
- private final SpeedControllerGroup m_rightMotors =
- new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
- new PWMVictorSPX(DriveConstants.kRightMotor2Port));
+ private final MotorControllerGroup m_rightMotors =
+ new MotorControllerGroup(
+ new PWMSparkMax(DriveConstants.kRightMotor1Port),
+ new PWMSparkMax(DriveConstants.kRightMotor2Port));
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
// The left-side drive encoder
private final Encoder m_leftEncoder =
- new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
- DriveConstants.kLeftEncoderReversed);
+ 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);
+ new Encoder(
+ DriveConstants.kRightEncoderPorts[0],
+ DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
- /**
- * Creates a new DriveSubsystem.
- */
+ /** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ // 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);
+
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -58,9 +63,7 @@
m_drive.arcadeDrive(fwd, rot);
}
- /**
- * Resets the drive encoders to currently read a position of 0.
- */
+ /** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
@@ -94,7 +97,7 @@
}
/**
- * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java
index 92e4979..fc508f3 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java
@@ -1,38 +1,31 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbotinlined.subsystems;
-import edu.wpi.first.wpilibj.DoubleSolenoid;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants;
-
import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
-/**
- * A hatch mechanism actuated by a single {@link edu.wpi.first.wpilibj.DoubleSolenoid}.
- */
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/** A hatch mechanism actuated by a single {@link edu.wpi.first.wpilibj.DoubleSolenoid}. */
public class HatchSubsystem extends SubsystemBase {
private final DoubleSolenoid m_hatchSolenoid =
- new DoubleSolenoid(HatchConstants.kHatchSolenoidModule, HatchConstants.kHatchSolenoidPorts[0],
- HatchConstants.kHatchSolenoidPorts[1]);
+ new DoubleSolenoid(
+ PneumaticsModuleType.CTREPCM,
+ HatchConstants.kHatchSolenoidPorts[0],
+ HatchConstants.kHatchSolenoidPorts[1]);
- /**
- * Grabs the hatch.
- */
+ /** Grabs the hatch. */
public void grabHatch() {
m_hatchSolenoid.set(kForward);
}
- /**
- * Releases the hatch.
- */
+ /** Releases the hatch. */
public void releaseHatch() {
m_hatchSolenoid.set(kReverse);
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java
index 2674b50..f02fa12 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbottraditional;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants. This class should not be used for any other purpose. All constants should be
- * declared globally (i.e. public static). Do not put anything functional in this class.
+ * 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.
@@ -22,8 +19,8 @@
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
- public static final int[] kLeftEncoderPorts = new int[]{0, 1};
- public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+ public static final int[] kRightEncoderPorts = new int[] {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
@@ -36,7 +33,7 @@
public static final class HatchConstants {
public static final int kHatchSolenoidModule = 0;
- public static final int[] kHatchSolenoidPorts = new int[]{0, 1};
+ public static final int[] kHatchSolenoidPorts = new int[] {0, 1};
}
public static final class AutoConstants {
@@ -46,6 +43,6 @@
}
public static final class OIConstants {
- public static final int kDriverControllerPort = 1;
+ public static final int kDriverControllerPort = 0;
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Main.java
index f09858c..0d084c3 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbottraditional;
@@ -15,8 +12,7 @@
* call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Robot.java
index 8747f35..f6bcdcd 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbottraditional;
@@ -37,8 +34,8 @@
* 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.
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
@@ -49,20 +46,14 @@
CommandScheduler.getInstance().run();
}
- /**
- * This function is called once each time the robot enters Disabled mode.
- */
+ /** This function is called once each time the robot enters Disabled mode. */
@Override
- public void disabledInit() {
- }
+ public void disabledInit() {}
@Override
- public void disabledPeriodic() {
- }
+ public void disabledPeriodic() {}
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
}
}
- /**
- * This function is called periodically during autonomous.
- */
+ /** This function is called periodically during autonomous. */
@Override
- public void autonomousPeriodic() {
- }
+ public void autonomousPeriodic() {}
@Override
public void teleopInit() {
@@ -98,12 +86,9 @@
}
}
- /**
- * This function is called periodically during operator control.
- */
+ /** This function is called periodically during operator control. */
@Override
- public void teleopPeriodic() {
- }
+ public void teleopPeriodic() {}
@Override
public void testInit() {
@@ -111,10 +96,7 @@
CommandScheduler.getInstance().cancelAll();
}
- /**
- * This function is called periodically during test mode.
- */
+ /** This function is called periodically during test mode. */
@Override
- public void testPeriodic() {
- }
+ public void testPeriodic() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java
index 0784dde..1f1024f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java
@@ -1,19 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbottraditional;
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.ComplexAuto;
@@ -24,14 +17,16 @@
import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.ReleaseHatch;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * 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.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems
@@ -42,8 +37,8 @@
// A simple auto routine that drives forward a specified distance, and then stops.
private final Command m_simpleAuto =
- new DriveDistance(AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed,
- m_robotDrive);
+ new DriveDistance(
+ AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed, m_robotDrive);
// A complex auto routine that drives forward, drops a hatch, and then drives backward.
private final Command m_complexAuto = new ComplexAuto(m_robotDrive, m_hatchSubsystem);
@@ -54,9 +49,7 @@
// The driver's controller
XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
- /**
- * The container for the robot. Contains subsystems, OI devices, and commands.
- */
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
@@ -67,9 +60,7 @@
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
new DefaultDrive(
- m_robotDrive,
- () -> m_driverController.getY(GenericHID.Hand.kLeft),
- () -> m_driverController.getX(GenericHID.Hand.kRight)));
+ m_robotDrive, m_driverController::getLeftY, m_driverController::getRightX));
// Add commands to the autonomous command chooser
m_chooser.setDefaultOption("Simple Auto", m_simpleAuto);
@@ -80,10 +71,10 @@
}
/**
- * Use this method to define your button->command mappings. Buttons can be created by
- * instantiating a {@link GenericHID} or one of its subclasses ({@link
- * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
- * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+ * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Grab the hatch when the 'A' button is pressed.
@@ -93,11 +84,10 @@
new JoystickButton(m_driverController, Button.kB.value)
.whenPressed(new ReleaseHatch(m_hatchSubsystem));
// While holding the shoulder button, drive at half speed
- new JoystickButton(m_driverController, Button.kBumperRight.value)
+ new JoystickButton(m_driverController, Button.kRightBumper.value)
.whenHeld(new HalveDriveSpeed(m_robotDrive));
}
-
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java
index 5f776a5..736a8d9 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java
@@ -1,21 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbottraditional.commands;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-
import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-/**
- * A complex auto command that drives forward, releases a hatch, and then drives backward.
- */
+/** A complex auto command that drives forward, releases a hatch, and then drives backward. */
public class ComplexAuto extends SequentialCommandGroup {
/**
* Creates a new ComplexAuto.
@@ -26,15 +20,14 @@
public ComplexAuto(DriveSubsystem drive, HatchSubsystem hatch) {
addCommands(
// Drive forward the specified distance
- new DriveDistance(AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed,
- drive),
+ new DriveDistance(
+ AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed, drive),
// Release the hatch
new ReleaseHatch(hatch),
// Drive backward the specified distance
- new DriveDistance(AutoConstants.kAutoBackupDistanceInches, -AutoConstants.kAutoDriveSpeed,
- drive));
+ new DriveDistance(
+ AutoConstants.kAutoBackupDistanceInches, -AutoConstants.kAutoDriveSpeed, drive));
}
-
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DefaultDrive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DefaultDrive.java
index 9399c30..460e787 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DefaultDrive.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DefaultDrive.java
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbottraditional.commands;
-import java.util.function.DoubleSupplier;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import java.util.function.DoubleSupplier;
/**
* A command to drive the robot with joystick input (passed in as {@link DoubleSupplier}s). Written
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java
index d4abd71..04a3c0b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java
@@ -1,15 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbottraditional.commands;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
public class DriveDistance extends CommandBase {
private final DriveSubsystem m_drive;
@@ -27,6 +23,7 @@
m_distance = inches;
m_speed = speed;
m_drive = drive;
+ addRequirements(m_drive);
}
@Override
@@ -36,6 +33,11 @@
}
@Override
+ public void execute() {
+ m_drive.arcadeDrive(m_speed, 0);
+ }
+
+ @Override
public void end(boolean interrupted) {
m_drive.arcadeDrive(0, 0);
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/GrabHatch.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/GrabHatch.java
index a30da45..9cd1550 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/GrabHatch.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/GrabHatch.java
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbottraditional.commands;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
import edu.wpi.first.wpilibj2.command.CommandBase;
-import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
-
/**
- * A simple command that grabs a hatch with the {@link HatchSubsystem}. Written explicitly for
- * pedagogical purposes. Actual code should inline a command this simple with {@link
+ * A simple command that grabs a hatch with the {@link HatchSubsystem}. Written explicitly for
+ * pedagogical purposes. Actual code should inline a command this simple with {@link
* edu.wpi.first.wpilibj2.command.InstantCommand}.
*/
public class GrabHatch extends CommandBase {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java
index bd6c093..c34d951 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java
@@ -1,15 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbottraditional.commands;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
public class HalveDriveSpeed extends CommandBase {
private final DriveSubsystem m_drive;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ReleaseHatch.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ReleaseHatch.java
index 1e6f0a0..6320486 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ReleaseHatch.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ReleaseHatch.java
@@ -1,19 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbottraditional.commands;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
-
-/**
- * A command that releases the hatch.
- */
+/** A command that releases the hatch. */
public class ReleaseHatch extends InstantCommand {
public ReleaseHatch(HatchSubsystem subsystem) {
super(subsystem::releaseHatch, subsystem);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
index f49abb9..d1236e2 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
@@ -1,48 +1,53 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbottraditional.subsystems;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
import 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 SpeedControllerGroup m_leftMotors =
- new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
- new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
+ private final MotorControllerGroup m_leftMotors =
+ new MotorControllerGroup(
+ new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+ new PWMSparkMax(DriveConstants.kLeftMotor2Port));
// The motors on the right side of the drive.
- private final SpeedControllerGroup m_rightMotors =
- new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
- new PWMVictorSPX(DriveConstants.kRightMotor2Port));
+ private final MotorControllerGroup m_rightMotors =
+ new MotorControllerGroup(
+ new PWMSparkMax(DriveConstants.kRightMotor1Port),
+ new PWMSparkMax(DriveConstants.kRightMotor2Port));
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
// The left-side drive encoder
private final Encoder m_leftEncoder =
- new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
- DriveConstants.kLeftEncoderReversed);
+ 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);
+ new Encoder(
+ DriveConstants.kRightEncoderPorts[0],
+ DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
- /**
- * Creates a new DriveSubsystem.
- */
+ /** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ // 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);
+
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -58,9 +63,7 @@
m_drive.arcadeDrive(fwd, rot);
}
- /**
- * Resets the drive encoders to currently read a position of 0.
- */
+ /** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
@@ -94,7 +97,7 @@
}
/**
- * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java
index 47da829..3526d17 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java
@@ -1,38 +1,31 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hatchbottraditional.subsystems;
-import edu.wpi.first.wpilibj.DoubleSolenoid;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants;
-
import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
-/**
- * A hatch mechanism actuated by a single {@link DoubleSolenoid}.
- */
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/** A hatch mechanism actuated by a single {@link DoubleSolenoid}. */
public class HatchSubsystem extends SubsystemBase {
private final DoubleSolenoid m_hatchSolenoid =
- new DoubleSolenoid(HatchConstants.kHatchSolenoidModule, HatchConstants.kHatchSolenoidPorts[0],
- HatchConstants.kHatchSolenoidPorts[1]);
+ new DoubleSolenoid(
+ PneumaticsModuleType.CTREPCM,
+ HatchConstants.kHatchSolenoidPorts[0],
+ HatchConstants.kHatchSolenoidPorts[1]);
- /**
- * Grabs the hatch.
- */
+ /** Grabs the hatch. */
public void grabHatch() {
m_hatchSolenoid.set(kForward);
}
- /**
- * Releases the hatch.
- */
+ /** Releases the hatch. */
public void releaseHatch() {
m_hatchSolenoid.set(kReverse);
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hidrumble/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hidrumble/Main.java
index 297bbf1..5d139ca 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hidrumble/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hidrumble/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hidrumble;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hidrumble/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hidrumble/Robot.java
old mode 100755
new mode 100644
index c7f6029..1c07516
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hidrumble/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hidrumble/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.hidrumble;
@@ -11,9 +8,7 @@
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
-/**
- * This is a demo program showing the use of GenericHID's rumble feature.
- */
+/** This is a demo program showing the use of GenericHID's rumble feature. */
public class Robot extends TimedRobot {
private final XboxController m_hid = new XboxController(0);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Main.java
index a0210b4..f2fb11f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.intermediatevision;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Robot.java
index 526e849..e6e8b65 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Robot.java
@@ -1,68 +1,64 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.intermediatevision;
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.CvSink;
+import edu.wpi.first.cscore.CvSource;
+import edu.wpi.first.cscore.UsbCamera;
+import edu.wpi.first.wpilibj.TimedRobot;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;
-import edu.wpi.cscore.CvSink;
-import edu.wpi.cscore.CvSource;
-import edu.wpi.cscore.UsbCamera;
-import edu.wpi.first.cameraserver.CameraServer;
-import edu.wpi.first.wpilibj.TimedRobot;
-
/**
- * This is a demo program showing the use of OpenCV to do vision processing. The
- * image is acquired from the USB camera, then a rectangle is put on the image
- * and sent to the dashboard. OpenCV has many methods for different types of
- * processing.
+ * This is a demo program showing the use of OpenCV to do vision processing. The image is acquired
+ * from the USB camera, then a rectangle is put on the image and sent to the dashboard. OpenCV has
+ * many methods for different types of processing.
*/
public class Robot extends TimedRobot {
Thread m_visionThread;
@Override
public void robotInit() {
- m_visionThread = new Thread(() -> {
- // Get the UsbCamera from CameraServer
- UsbCamera camera = CameraServer.getInstance().startAutomaticCapture();
- // Set the resolution
- camera.setResolution(640, 480);
+ m_visionThread =
+ new Thread(
+ () -> {
+ // Get the UsbCamera from CameraServer
+ UsbCamera camera = CameraServer.startAutomaticCapture();
+ // Set the resolution
+ camera.setResolution(640, 480);
- // Get a CvSink. This will capture Mats from the camera
- CvSink cvSink = CameraServer.getInstance().getVideo();
- // Setup a CvSource. This will send images back to the Dashboard
- CvSource outputStream
- = CameraServer.getInstance().putVideo("Rectangle", 640, 480);
+ // Get a CvSink. This will capture Mats from the camera
+ CvSink cvSink = CameraServer.getVideo();
+ // Setup a CvSource. This will send images back to the Dashboard
+ CvSource outputStream = CameraServer.putVideo("Rectangle", 640, 480);
- // Mats are very memory expensive. Lets reuse this Mat.
- Mat mat = new Mat();
+ // Mats are very memory expensive. Lets reuse this Mat.
+ Mat mat = new Mat();
- // This cannot be 'true'. The program will never exit if it is. This
- // lets the robot stop this thread when restarting robot code or
- // deploying.
- while (!Thread.interrupted()) {
- // Tell the CvSink to grab a frame from the camera and put it
- // in the source mat. If there is an error notify the output.
- if (cvSink.grabFrame(mat) == 0) {
- // Send the output the error.
- outputStream.notifyError(cvSink.getError());
- // skip the rest of the current iteration
- continue;
- }
- // Put a rectangle on the image
- Imgproc.rectangle(mat, new Point(100, 100), new Point(400, 400),
- new Scalar(255, 255, 255), 5);
- // Give the output stream a new image to display
- outputStream.putFrame(mat);
- }
- });
+ // This cannot be 'true'. The program will never exit if it is. This
+ // lets the robot stop this thread when restarting robot code or
+ // deploying.
+ while (!Thread.interrupted()) {
+ // Tell the CvSink to grab a frame from the camera and put it
+ // in the source mat. If there is an error notify the output.
+ if (cvSink.grabFrame(mat) == 0) {
+ // Send the output the error.
+ outputStream.notifyError(cvSink.getError());
+ // skip the rest of the current iteration
+ continue;
+ }
+ // Put a rectangle on the image
+ Imgproc.rectangle(
+ mat, new Point(100, 100), new Point(400, 400), new Scalar(255, 255, 255), 5);
+ // Give the output stream a new image to display
+ outputStream.putFrame(mat);
+ }
+ });
m_visionThread.setDaemon(true);
m_visionThread.start();
}
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 2a081ff..c660ae2 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
@@ -1,36 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.mecanumbot;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
-/**
- * Represents a mecanum drive style drivetrain.
- */
-@SuppressWarnings("PMD.TooManyFields")
+/** Represents a mecanum drive style drivetrain. */
public class Drivetrain {
public static final double kMaxSpeed = 3.0; // 3 meters per second
public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
- private final SpeedController m_frontLeftMotor = new PWMVictorSPX(1);
- private final SpeedController m_frontRightMotor = new PWMVictorSPX(2);
- private final SpeedController m_backLeftMotor = new PWMVictorSPX(3);
- private final SpeedController m_backRightMotor = new PWMVictorSPX(4);
+ 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 Encoder m_frontLeftEncoder = new Encoder(0, 1);
private final Encoder m_frontRightEncoder = new Encoder(2, 3);
@@ -49,19 +43,17 @@
private final AnalogGyro m_gyro = new AnalogGyro(0);
- private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(
- m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
- );
+ private final MecanumDriveKinematics m_kinematics =
+ new MecanumDriveKinematics(
+ m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
- private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
- m_gyro.getRotation2d());
+ private final MecanumDriveOdometry m_odometry =
+ new MecanumDriveOdometry(m_kinematics, m_gyro.getRotation2d());
// Gains are for example purposes only - must be determined for your own robot!
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
- /**
- * Constructs a MecanumDrive and resets the gyro.
- */
+ /** Constructs a MecanumDrive and resets the gyro. */
public Drivetrain() {
m_gyro.reset();
}
@@ -76,8 +68,7 @@
m_frontLeftEncoder.getRate(),
m_frontRightEncoder.getRate(),
m_backLeftEncoder.getRate(),
- m_backRightEncoder.getRate()
- );
+ m_backRightEncoder.getRate());
}
/**
@@ -91,18 +82,18 @@
final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond);
final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond);
- final double frontLeftOutput = m_frontLeftPIDController.calculate(
- m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond
- );
- final double frontRightOutput = m_frontRightPIDController.calculate(
- m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond
- );
- final double backLeftOutput = m_backLeftPIDController.calculate(
- m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond
- );
- final double backRightOutput = m_backRightPIDController.calculate(
- m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond
- );
+ final double frontLeftOutput =
+ m_frontLeftPIDController.calculate(
+ m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond);
+ final double frontRightOutput =
+ m_frontRightPIDController.calculate(
+ m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond);
+ final double backLeftOutput =
+ m_backLeftPIDController.calculate(
+ m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond);
+ final double backRightOutput =
+ m_backRightPIDController.calculate(
+ m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond);
m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward);
m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward);
@@ -113,25 +104,23 @@
/**
* Method to drive the robot using joystick info.
*
- * @param xSpeed Speed of the robot in the x direction (forward).
- * @param ySpeed Speed of the robot in the y direction (sideways).
- * @param rot Angular rate of the robot.
+ * @param xSpeed Speed of the robot in the x direction (forward).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
*/
@SuppressWarnings("ParameterName")
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
- var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(
- fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
- xSpeed, ySpeed, rot, m_gyro.getRotation2d()
- ) : new ChassisSpeeds(xSpeed, ySpeed, rot)
- );
+ var mecanumDriveWheelSpeeds =
+ m_kinematics.toWheelSpeeds(
+ fieldRelative
+ ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+ : new ChassisSpeeds(xSpeed, ySpeed, rot));
mecanumDriveWheelSpeeds.normalize(kMaxSpeed);
setSpeeds(mecanumDriveWheelSpeeds);
}
- /**
- * Updates the field relative position of the robot.
- */
+ /** Updates the field relative position of the robot. */
public void updateOdometry() {
m_odometry.update(m_gyro.getRotation2d(), getCurrentState());
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Main.java
index 27a4b32..338365e 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.mecanumbot;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
index d41a947..e1d9a08 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
@@ -1,14 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.mecanumbot;
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.SlewRateLimiter;
+import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
@@ -35,24 +31,18 @@
private void driveWithJoystick(boolean fieldRelative) {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
- final var xSpeed =
- -m_xspeedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
- * Drivetrain.kMaxSpeed;
+ final var xSpeed = -m_xspeedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
// Get the y speed or sideways/strafe speed. We are inverting this because
// we want a positive value when we pull to the left. Xbox controllers
// return positive values when you pull to the right by default.
- final var ySpeed =
- -m_yspeedLimiter.calculate(m_controller.getX(GenericHID.Hand.kLeft))
- * Drivetrain.kMaxSpeed;
+ final var ySpeed = -m_yspeedLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxSpeed;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
- final var rot =
- -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
- * Drivetrain.kMaxAngularSpeed;
+ final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative);
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java
index b402109..e959ac2 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java
@@ -1,21 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.mecanumcontrollercommand;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
/**
* 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.
+ * 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.
@@ -27,10 +24,10 @@
public static final int kFrontRightMotorPort = 2;
public static final int kRearRightMotorPort = 3;
- public static final int[] kFrontLeftEncoderPorts = new int[]{0, 1};
- public static final int[] kRearLeftEncoderPorts = new int[]{2, 3};
- public static final int[] kFrontRightEncoderPorts = new int[]{4, 5};
- public static final int[] kRearRightEncoderPorts = new int[]{5, 6};
+ public static final int[] kFrontLeftEncoderPorts = new int[] {0, 1};
+ public static final int[] kRearLeftEncoderPorts = new int[] {2, 3};
+ public static final int[] kFrontRightEncoderPorts = new int[] {4, 5};
+ public static final int[] kRearRightEncoderPorts = new int[] {6, 7};
public static final boolean kFrontLeftEncoderReversed = false;
public static final boolean kRearLeftEncoderReversed = true;
@@ -44,10 +41,10 @@
public static final MecanumDriveKinematics kDriveKinematics =
new MecanumDriveKinematics(
- new Translation2d(kWheelBase / 2, kTrackWidth / 2),
- new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
- new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
- new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+ new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterMeters = 0.15;
@@ -68,12 +65,10 @@
public static final double kPRearLeftVel = 0.5;
public static final double kPFrontRightVel = 0.5;
public static final double kPRearRightVel = 0.5;
-
}
public static final class OIConstants {
- public static final int kDriverControllerPort = 1;
-
+ public static final int kDriverControllerPort = 0;
}
public static final class AutoConstants {
@@ -86,10 +81,9 @@
public static final double kPYController = 0.5;
public static final double kPThetaController = 0.5;
- //Constraint for the motion profilied robot angle controller
+ // Constraint for the motion profilied robot angle controller
public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
- new TrapezoidProfile.Constraints(kMaxAngularSpeedRadiansPerSecond,
- kMaxAngularSpeedRadiansPerSecondSquared);
-
+ new TrapezoidProfile.Constraints(
+ kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java
index 9670d3d..3398c21 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.mecanumcontrollercommand;
@@ -15,8 +12,7 @@
* call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java
index 6f2ccbf..3913f5d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.mecanumcontrollercommand;
@@ -37,8 +34,8 @@
* 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.
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
@@ -49,20 +46,14 @@
CommandScheduler.getInstance().run();
}
- /**
- * This function is called once each time the robot enters Disabled mode.
- */
+ /** This function is called once each time the robot enters Disabled mode. */
@Override
- public void disabledInit() {
- }
+ public void disabledInit() {}
@Override
- public void disabledPeriodic() {
- }
+ public void disabledPeriodic() {}
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
}
}
- /**
- * This function is called periodically during autonomous.
- */
+ /** This function is called periodically during autonomous. */
@Override
- public void autonomousPeriodic() {
- }
+ public void autonomousPeriodic() {}
@Override
public void teleopInit() {
@@ -98,13 +86,9 @@
}
}
- /**
- * This function is called periodically during operator control.
- */
+ /** This function is called periodically during operator control. */
@Override
- public void teleopPeriodic() {
-
- }
+ public void teleopPeriodic() {}
@Override
public void testInit() {
@@ -112,10 +96,7 @@
CommandScheduler.getInstance().cancelAll();
}
- /**
- * This function is called periodically during test mode.
- */
+ /** This function is called periodically during test mode. */
@Override
- public void testPeriodic() {
- }
+ public void testPeriodic() {}
}
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 91055c6..0d36a94 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
@@ -1,34 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.mecanumcontrollercommand;
-import java.util.List;
-
-import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.XboxController.Button;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.MecanumControllerCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants;
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants;
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.MecanumControllerCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import java.util.List;
/*
* This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -36,7 +30,6 @@
* periodic methods (other than the scheduler calls). Instead, the structure of the robot
* (including subsystems, commands, and button mappings) should be declared here.
*/
-@SuppressWarnings("PMD.ExcessiveImports")
public class RobotContainer {
// The robot's subsystems
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
@@ -44,9 +37,7 @@
// The driver's controller
XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
- /**
- * The container for the robot. Contains subsystems, OI devices, and commands.
- */
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
@@ -56,25 +47,27 @@
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
- new RunCommand(() -> m_robotDrive.drive(
- m_driverController.getY(GenericHID.Hand.kLeft),
- m_driverController.getX(GenericHID.Hand.kRight),
- m_driverController.getX(GenericHID.Hand.kLeft), false)));
-
+ new RunCommand(
+ () ->
+ m_robotDrive.drive(
+ m_driverController.getLeftY(),
+ m_driverController.getRightX(),
+ m_driverController.getLeftX(),
+ false),
+ m_robotDrive));
}
/**
- * Use this method to define your button->command mappings. Buttons can be created by
- * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
* {@link JoystickButton}.
*/
private void configureButtonBindings() {
// Drive at half speed when the right bumper is held
- new JoystickButton(m_driverController, Button.kBumperRight.value)
+ new JoystickButton(m_driverController, Button.kRightBumper.value)
.whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
-
}
/**
@@ -85,53 +78,47 @@
public Command getAutonomousCommand() {
// Create config for trajectory
TrajectoryConfig config =
- new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
- AutoConstants.kMaxAccelerationMetersPerSecondSquared)
+ new TrajectoryConfig(
+ AutoConstants.kMaxSpeedMetersPerSecond,
+ AutoConstants.kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(DriveConstants.kDriveKinematics);
// An example trajectory to follow. All units in meters.
- Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
- // Start at the origin facing the +X direction
- new Pose2d(0, 0, new Rotation2d(0)),
- // Pass through these two interior waypoints, making an 's' curve path
- List.of(
- new Translation2d(1, 1),
- new Translation2d(2, -1)
- ),
- // End 3 meters straight ahead of where we started, facing forward
- new Pose2d(3, 0, new Rotation2d(0)),
- config
- );
+ Trajectory exampleTrajectory =
+ TrajectoryGenerator.generateTrajectory(
+ // Start at the origin facing the +X direction
+ new Pose2d(0, 0, new Rotation2d(0)),
+ // Pass through these two interior waypoints, making an 's' curve path
+ List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
+ // End 3 meters straight ahead of where we started, facing forward
+ new Pose2d(3, 0, new Rotation2d(0)),
+ config);
- MecanumControllerCommand mecanumControllerCommand = new MecanumControllerCommand(
- exampleTrajectory,
- m_robotDrive::getPose,
+ MecanumControllerCommand mecanumControllerCommand =
+ new MecanumControllerCommand(
+ exampleTrajectory,
+ m_robotDrive::getPose,
+ DriveConstants.kFeedforward,
+ DriveConstants.kDriveKinematics,
- DriveConstants.kFeedforward,
- DriveConstants.kDriveKinematics,
+ // Position contollers
+ new PIDController(AutoConstants.kPXController, 0, 0),
+ new PIDController(AutoConstants.kPYController, 0, 0),
+ new ProfiledPIDController(
+ AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints),
- //Position contollers
- new PIDController(AutoConstants.kPXController, 0, 0),
- new PIDController(AutoConstants.kPYController, 0, 0),
- new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0,
- AutoConstants.kThetaControllerConstraints),
+ // Needed for normalizing wheel speeds
+ AutoConstants.kMaxSpeedMetersPerSecond,
- //Needed for normalizing wheel speeds
- AutoConstants.kMaxSpeedMetersPerSecond,
-
- //Velocity PID's
- new PIDController(DriveConstants.kPFrontLeftVel, 0, 0),
- new PIDController(DriveConstants.kPRearLeftVel, 0, 0),
- new PIDController(DriveConstants.kPFrontRightVel, 0, 0),
- new PIDController(DriveConstants.kPRearRightVel, 0, 0),
-
- m_robotDrive::getCurrentWheelSpeeds,
-
- m_robotDrive::setDriveSpeedControllersVolts, //Consumer for the output motor voltages
-
- m_robotDrive
- );
+ // Velocity PID's
+ new PIDController(DriveConstants.kPFrontLeftVel, 0, 0),
+ new PIDController(DriveConstants.kPRearLeftVel, 0, 0),
+ new PIDController(DriveConstants.kPFrontRightVel, 0, 0),
+ new PIDController(DriveConstants.kPRearRightVel, 0, 0),
+ m_robotDrive::getCurrentWheelSpeeds,
+ 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());
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 1fac67e..23da867 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
@@ -1,60 +1,57 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.mecanumcontrollercommand.subsystems;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages;
+import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants;
import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants;
-
public class DriveSubsystem extends SubsystemBase {
- private final PWMVictorSPX m_frontLeft = new PWMVictorSPX(DriveConstants.kFrontLeftMotorPort);
- private final PWMVictorSPX m_rearLeft = new PWMVictorSPX(DriveConstants.kRearLeftMotorPort);
- private final PWMVictorSPX m_frontRight = new PWMVictorSPX(DriveConstants.kFrontRightMotorPort);
- private final PWMVictorSPX m_rearRight = new PWMVictorSPX(DriveConstants.kRearRightMotorPort);
+ private final PWMSparkMax m_frontLeft = new PWMSparkMax(DriveConstants.kFrontLeftMotorPort);
+ private final PWMSparkMax m_rearLeft = new PWMSparkMax(DriveConstants.kRearLeftMotorPort);
+ private final PWMSparkMax m_frontRight = new PWMSparkMax(DriveConstants.kFrontRightMotorPort);
+ private final PWMSparkMax m_rearRight = new PWMSparkMax(DriveConstants.kRearRightMotorPort);
- private final MecanumDrive m_drive = new MecanumDrive(
- m_frontLeft,
- m_rearLeft,
- m_frontRight,
- m_rearRight);
+ private final MecanumDrive m_drive =
+ new MecanumDrive(m_frontLeft, m_rearLeft, m_frontRight, m_rearRight);
// The front-left-side drive encoder
private final Encoder m_frontLeftEncoder =
- new Encoder(DriveConstants.kFrontLeftEncoderPorts[0],
- DriveConstants.kFrontLeftEncoderPorts[1],
- DriveConstants.kFrontLeftEncoderReversed);
+ new Encoder(
+ DriveConstants.kFrontLeftEncoderPorts[0],
+ DriveConstants.kFrontLeftEncoderPorts[1],
+ DriveConstants.kFrontLeftEncoderReversed);
// The rear-left-side drive encoder
private final Encoder m_rearLeftEncoder =
- new Encoder(DriveConstants.kRearLeftEncoderPorts[0],
- DriveConstants.kRearLeftEncoderPorts[1],
- DriveConstants.kRearLeftEncoderReversed);
+ new Encoder(
+ DriveConstants.kRearLeftEncoderPorts[0],
+ DriveConstants.kRearLeftEncoderPorts[1],
+ DriveConstants.kRearLeftEncoderReversed);
// The front-right--side drive encoder
private final Encoder m_frontRightEncoder =
- new Encoder(DriveConstants.kFrontRightEncoderPorts[0],
- DriveConstants.kFrontRightEncoderPorts[1],
- DriveConstants.kFrontRightEncoderReversed);
+ new Encoder(
+ DriveConstants.kFrontRightEncoderPorts[0],
+ DriveConstants.kFrontRightEncoderPorts[1],
+ DriveConstants.kFrontRightEncoderReversed);
// The rear-right-side drive encoder
private final Encoder m_rearRightEncoder =
- new Encoder(DriveConstants.kRearRightEncoderPorts[0],
- DriveConstants.kRearRightEncoderPorts[1],
- DriveConstants.kRearRightEncoderReversed);
+ new Encoder(
+ DriveConstants.kRearRightEncoderPorts[0],
+ DriveConstants.kRearRightEncoderPorts[1],
+ DriveConstants.kRearRightEncoderReversed);
// The gyro sensor
private final Gyro m_gyro = new ADXRS450_Gyro();
@@ -63,9 +60,7 @@
MecanumDriveOdometry m_odometry =
new MecanumDriveOdometry(DriveConstants.kDriveKinematics, m_gyro.getRotation2d());
- /**
- * Creates a new DriveSubsystem.
- */
+ /** Creates a new DriveSubsystem. */
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
m_frontLeftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -77,12 +72,13 @@
@Override
public void periodic() {
// Update the odometry in the periodic block
- m_odometry.update(m_gyro.getRotation2d(),
- new MecanumDriveWheelSpeeds(
- m_frontLeftEncoder.getRate(),
- m_rearLeftEncoder.getRate(),
- m_frontRightEncoder.getRate(),
- m_rearRightEncoder.getRate()));
+ m_odometry.update(
+ m_gyro.getRotation2d(),
+ new MecanumDriveWheelSpeeds(
+ m_frontLeftEncoder.getRate(),
+ m_rearLeftEncoder.getRate(),
+ m_frontRightEncoder.getRate(),
+ m_rearRightEncoder.getRate()));
}
/**
@@ -107,9 +103,9 @@
* Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1] and the linear
* speeds have no effect on the angular speed.
*
- * @param xSpeed Speed of the robot in the x direction (forward/backwards).
- * @param ySpeed Speed of the robot in the y direction (sideways).
- * @param rot Angular rate of the robot.
+ * @param xSpeed Speed of the robot in the x direction (forward/backwards).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
*/
@SuppressWarnings("ParameterName")
@@ -119,23 +115,17 @@
} else {
m_drive.driveCartesian(ySpeed, xSpeed, rot);
}
-
}
- /**
- * Sets the front left drive SpeedController to a voltage.
- */
- public void setDriveSpeedControllersVolts(MecanumDriveMotorVoltages volts) {
+ /** Sets the front left drive MotorController to a voltage. */
+ public void setDriveMotorControllersVolts(MecanumDriveMotorVoltages volts) {
m_frontLeft.setVoltage(volts.frontLeftVoltage);
m_rearLeft.setVoltage(volts.rearLeftVoltage);
m_frontRight.setVoltage(volts.frontRightVoltage);
m_rearRight.setVoltage(volts.rearRightVoltage);
}
-
- /**
- * Resets the drive encoders to currently read a position of 0.
- */
+ /** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_frontLeftEncoder.reset();
m_rearLeftEncoder.reset();
@@ -148,7 +138,6 @@
*
* @return the front left drive encoder
*/
-
public Encoder getFrontLeftEncoder() {
return m_frontLeftEncoder;
}
@@ -158,7 +147,6 @@
*
* @return the rear left drive encoder
*/
-
public Encoder getRearLeftEncoder() {
return m_rearLeftEncoder;
}
@@ -168,7 +156,6 @@
*
* @return the front right drive encoder
*/
-
public Encoder getFrontRightEncoder() {
return m_frontRightEncoder;
}
@@ -178,7 +165,6 @@
*
* @return the rear right encoder
*/
-
public Encoder getRearRightEncoder() {
return m_rearRightEncoder;
}
@@ -188,15 +174,14 @@
*
* @return the current wheel speeds in a MecanumDriveWheelSpeeds object.
*/
-
public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
- return new MecanumDriveWheelSpeeds(m_frontLeftEncoder.getRate(),
- m_rearLeftEncoder.getRate(),
- m_frontRightEncoder.getRate(),
- m_rearRightEncoder.getRate());
+ return new MecanumDriveWheelSpeeds(
+ m_frontLeftEncoder.getRate(),
+ m_rearLeftEncoder.getRate(),
+ m_frontRightEncoder.getRate(),
+ m_rearRightEncoder.getRate());
}
-
/**
* Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
@@ -206,9 +191,7 @@
m_drive.setMaxOutput(maxOutput);
}
- /**
- * Zeroes the heading of the robot.
- */
+ /** Zeroes the heading of the robot. */
public void zeroHeading() {
m_gyro.reset();
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Main.java
index 4a8f2e1..b44186c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.mecanumdrive;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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
old mode 100755
new mode 100644
index 35c975c..80c16b5
--- 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
@@ -1,21 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.mecanumdrive;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
-/**
- * This is a demo program showing how to use Mecanum control with the RobotDrive
- * class.
- */
+/** This is a demo program showing how to use Mecanum control with the MecanumDrive class. */
public class Robot extends TimedRobot {
private static final int kFrontLeftChannel = 2;
private static final int kRearLeftChannel = 3;
@@ -29,10 +23,10 @@
@Override
public void robotInit() {
- PWMVictorSPX frontLeft = new PWMVictorSPX(kFrontLeftChannel);
- PWMVictorSPX rearLeft = new PWMVictorSPX(kRearLeftChannel);
- PWMVictorSPX frontRight = new PWMVictorSPX(kFrontRightChannel);
- PWMVictorSPX rearRight = new PWMVictorSPX(kRearRightChannel);
+ PWMSparkMax frontLeft = new PWMSparkMax(kFrontLeftChannel);
+ PWMSparkMax rearLeft = new PWMSparkMax(kRearLeftChannel);
+ PWMSparkMax frontRight = new PWMSparkMax(kFrontRightChannel);
+ PWMSparkMax rearRight = new PWMSparkMax(kRearRightChannel);
// Invert the left side motors.
// You may need to change or remove this to match your robot.
@@ -48,7 +42,6 @@
public void teleopPeriodic() {
// Use the joystick X axis for lateral movement, Y axis for forward
// movement, and Z axis for rotation.
- m_robotDrive.driveCartesian(m_stick.getX(), m_stick.getY(),
- m_stick.getZ(), 0.0);
+ m_robotDrive.driveCartesian(m_stick.getX(), m_stick.getY(), m_stick.getZ(), 0.0);
}
}
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
new file mode 100644
index 0000000..ba544b2
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
@@ -0,0 +1,146 @@
+// 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.mecanumdriveposeestimator;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.estimator.MecanumDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.math.util.Units;
+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. */
+public class Drivetrain {
+ public static final double kMaxSpeed = 3.0; // 3 meters per second
+ public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
+
+ private final 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 Encoder m_frontLeftEncoder = new Encoder(0, 1);
+ private final Encoder m_frontRightEncoder = new Encoder(2, 3);
+ private final Encoder m_backLeftEncoder = new Encoder(4, 5);
+ private final Encoder m_backRightEncoder = new Encoder(6, 7);
+
+ private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
+ private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
+ private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
+ private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
+
+ private final PIDController m_frontLeftPIDController = new PIDController(1, 0, 0);
+ private final PIDController m_frontRightPIDController = new PIDController(1, 0, 0);
+ private final PIDController m_backLeftPIDController = new PIDController(1, 0, 0);
+ private final PIDController m_backRightPIDController = new PIDController(1, 0, 0);
+
+ private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+ private final MecanumDriveKinematics m_kinematics =
+ new MecanumDriveKinematics(
+ m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
+
+ /* Here we use MecanumDrivePoseEstimator so that we can fuse odometry readings. The numbers used
+ below are robot specific, and should be tuned. */
+ private final MecanumDrivePoseEstimator m_poseEstimator =
+ new MecanumDrivePoseEstimator(
+ m_gyro.getRotation2d(),
+ new Pose2d(),
+ m_kinematics,
+ VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
+ VecBuilder.fill(Units.degreesToRadians(0.01)),
+ VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+
+ // Gains are for example purposes only - must be determined for your own robot!
+ private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
+
+ /** Constructs a MecanumDrive and resets the gyro. */
+ public Drivetrain() {
+ m_gyro.reset();
+ }
+
+ /**
+ * Returns the current state of the drivetrain.
+ *
+ * @return The current state of the drivetrain.
+ */
+ public MecanumDriveWheelSpeeds getCurrentState() {
+ return new MecanumDriveWheelSpeeds(
+ m_frontLeftEncoder.getRate(),
+ m_frontRightEncoder.getRate(),
+ m_backLeftEncoder.getRate(),
+ m_backRightEncoder.getRate());
+ }
+
+ /**
+ * Set the desired speeds for each wheel.
+ *
+ * @param speeds The desired wheel speeds.
+ */
+ public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
+ final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond);
+ final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond);
+ final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond);
+ final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond);
+
+ final double frontLeftOutput =
+ m_frontLeftPIDController.calculate(
+ m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond);
+ final double frontRightOutput =
+ m_frontRightPIDController.calculate(
+ m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond);
+ final double backLeftOutput =
+ m_backLeftPIDController.calculate(
+ m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond);
+ final double backRightOutput =
+ m_backRightPIDController.calculate(
+ m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond);
+
+ m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward);
+ m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward);
+ m_backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward);
+ m_backRightMotor.setVoltage(backRightOutput + backRightFeedforward);
+ }
+
+ /**
+ * Method to drive the robot using joystick info.
+ *
+ * @param xSpeed Speed of the robot in the x direction (forward).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
+ * @param fieldRelative Whether the provided x and y speeds are relative to the field.
+ */
+ @SuppressWarnings("ParameterName")
+ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+ var mecanumDriveWheelSpeeds =
+ m_kinematics.toWheelSpeeds(
+ fieldRelative
+ ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+ : new ChassisSpeeds(xSpeed, ySpeed, rot));
+ mecanumDriveWheelSpeeds.normalize(kMaxSpeed);
+ setSpeeds(mecanumDriveWheelSpeeds);
+ }
+
+ /** Updates the field relative position of the robot. */
+ public void updateOdometry() {
+ m_poseEstimator.update(m_gyro.getRotation2d(), getCurrentState());
+
+ // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
+ // a real robot, this must be calculated based either on latency or timestamps.
+ m_poseEstimator.addVisionMeasurement(
+ ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
+ m_poseEstimator.getEstimatedPosition()),
+ Timer.getFPGATimestamp() - 0.3);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/ExampleGlobalMeasurementSensor.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/ExampleGlobalMeasurementSensor.java
new file mode 100644
index 0000000..8b7a635
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/ExampleGlobalMeasurementSensor.java
@@ -0,0 +1,32 @@
+// 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.mecanumdriveposeestimator;
+
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+
+/** This dummy class represents a global measurement sensor, such as a computer vision solution. */
+public final class ExampleGlobalMeasurementSensor {
+ private ExampleGlobalMeasurementSensor() {
+ // Utility class
+ }
+
+ /**
+ * Get a "noisy" fake global pose reading.
+ *
+ * @param estimatedRobotPose The robot pose.
+ */
+ public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
+ var rand =
+ StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+ return new Pose2d(
+ estimatedRobotPose.getX() + rand.get(0, 0),
+ estimatedRobotPose.getY() + rand.get(1, 0),
+ estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0))));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Main.java
new file mode 100644
index 0000000..a6c5ccc
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/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.mecanumdriveposeestimator;
+
+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/mecanumdriveposeestimator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java
new file mode 100644
index 0000000..fb22388
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java
@@ -0,0 +1,49 @@
+// 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.mecanumdriveposeestimator;
+
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class Robot extends TimedRobot {
+ private final XboxController m_controller = new XboxController(0);
+ private final Drivetrain m_mecanum = new Drivetrain();
+
+ // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+ private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3);
+ private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3);
+ private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
+ @Override
+ public void autonomousPeriodic() {
+ driveWithJoystick(false);
+ m_mecanum.updateOdometry();
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ driveWithJoystick(true);
+ }
+
+ private void driveWithJoystick(boolean fieldRelative) {
+ // Get the x speed. We are inverting this because Xbox controllers return
+ // negative values when we push forward.
+ final var xSpeed = -m_xspeedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
+
+ // Get the y speed or sideways/strafe speed. We are inverting this because
+ // we want a positive value when we pull to the left. Xbox controllers
+ // return positive values when you pull to the right by default.
+ final var ySpeed = -m_yspeedLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxSpeed;
+
+ // Get the rate of angular rotation. We are inverting this because we want a
+ // positive value when we pull to the left (remember, CCW is positive in
+ // mathematics). Xbox controllers return positive values when you pull to
+ // the right by default.
+ final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
+
+ m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Main.java
new file mode 100644
index 0000000..a477a03
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/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.mechanism2d;
+
+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/mechanism2d/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Robot.java
new file mode 100644
index 0000000..264b8ad
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Robot.java
@@ -0,0 +1,71 @@
+// 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.mechanism2d;
+
+import edu.wpi.first.wpilibj.AnalogPotentiometer;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+/**
+ * This sample program shows how to use Mechanism2d - a visual representation of arms, elevators,
+ * and other mechanisms on dashboards; driven by a node-based API.
+ *
+ * <p>Ligaments are based on other ligaments or roots, and roots are contained in the base
+ * Mechanism2d object.
+ */
+public class Robot extends TimedRobot {
+ private static final double kMetersPerPulse = 0.01;
+ private static final double kElevatorMinimumLength = 0.5;
+
+ private final PWMSparkMax m_elevatorMotor = new PWMSparkMax(0);
+ private final PWMSparkMax m_wristMotor = new PWMSparkMax(1);
+ private final AnalogPotentiometer m_wristPot = new AnalogPotentiometer(1, 90);
+ private final Encoder m_elevatorEncoder = new Encoder(0, 1);
+ private final Joystick m_joystick = new Joystick(0);
+
+ private MechanismLigament2d m_elevator;
+ private MechanismLigament2d m_wrist;
+
+ @Override
+ public void robotInit() {
+ m_elevatorEncoder.setDistancePerPulse(kMetersPerPulse);
+
+ // the main mechanism object
+ Mechanism2d mech = new Mechanism2d(3, 3);
+ // the mechanism root node
+ MechanismRoot2d root = mech.getRoot("climber", 2, 0);
+
+ // MechanismLigament2d objects represent each "section"/"stage" of the mechanism, and are based
+ // off the root node or another ligament object
+ m_elevator = root.append(new MechanismLigament2d("elevator", kElevatorMinimumLength, 90));
+ m_wrist =
+ m_elevator.append(
+ new MechanismLigament2d("wrist", 0.5, 90, 6, new Color8Bit(Color.kPurple)));
+
+ // post the mechanism to the dashboard
+ SmartDashboard.putData("Mech2d", mech);
+ }
+
+ @Override
+ public void robotPeriodic() {
+ // update the dashboard mechanism's state
+ m_elevator.setLength(kElevatorMinimumLength + m_elevatorEncoder.getDistance());
+ m_wrist.setAngle(m_wristPot.get());
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ m_elevatorMotor.set(m_joystick.getRawAxis(0));
+ m_wristMotor.set(m_joystick.getRawAxis(1));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Main.java
index f817e8f..abd3c94 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.motorcontrol;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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
old mode 100755
new mode 100644
index 7dfb49b..9976381
--- 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
@@ -1,35 +1,31 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.motorcontrol;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
- * This sample program shows how to control a motor using a joystick. In the
- * operator control part of the program, the joystick is read and the value is
- * written to the motor.
+ * This sample program shows how to control a motor using a joystick. In the operator control part
+ * of the program, the joystick is read and the value is written to the motor.
*
- * <p>Joystick analog values range from -1 to 1 and speed controller inputs also
- * range from -1 to 1 making it easy to work together.
+ * <p>Joystick analog values range from -1 to 1 and speed controller inputs also range from -1 to 1
+ * making it easy to work together.
*/
public class Robot extends TimedRobot {
private static final int kMotorPort = 0;
private static final int kJoystickPort = 0;
- private SpeedController m_motor;
+ private MotorController m_motor;
private Joystick m_joystick;
@Override
public void robotInit() {
- m_motor = new PWMVictorSPX(kMotorPort);
+ m_motor = new PWMSparkMax(kMotorPort);
m_joystick = new Joystick(kJoystickPort);
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrolencoder/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrolencoder/Main.java
index 5b03eed..0ca48f6 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrolencoder/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrolencoder/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.motorcontrolencoder;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrolencoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrolencoder/Robot.java
index b0cc217..9b68965 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrolencoder/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrolencoder/Robot.java
@@ -1,29 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.motorcontrolencoder;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
- * This sample program shows how to control a motor using a joystick. In the
- * operator control part of the program, the joystick is read and the value is
- * written to the motor.
+ * This sample program shows how to control a motor using a joystick. In the operator control part
+ * of the program, the joystick is read and the value is written to the motor.
*
- * <p>Joystick analog values range from -1 to 1 and speed controller inputs also
- * range from -1 to 1 making it easy to work together.
+ * <p>Joystick analog values range from -1 to 1 and speed controller inputs also range from -1 to 1
+ * making it easy to work together.
*
- * <p>In addition, the encoder value of an encoder connected to ports 0 and 1 is
- * consistently sent to the Dashboard.
+ * <p>In addition, the encoder value of an encoder connected to ports 0 and 1 is consistently sent
+ * to the Dashboard.
*/
public class Robot extends TimedRobot {
private static final int kMotorPort = 0;
@@ -31,13 +27,13 @@
private static final int kEncoderPortA = 0;
private static final int kEncoderPortB = 1;
- private SpeedController m_motor;
+ private MotorController m_motor;
private Joystick m_joystick;
private Encoder m_encoder;
@Override
public void robotInit() {
- m_motor = new PWMVictorSPX(kMotorPort);
+ m_motor = new PWMSparkMax(kMotorPort);
m_joystick = new Joystick(kJoystickPort);
m_encoder = new Encoder(kEncoderPortA, kEncoderPortB);
// Use SetDistancePerPulse to set the multiplier for GetDistance
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Main.java
deleted file mode 100644
index 04a26be..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Main.java
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat;
-
-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/pacgoat/OI.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/OI.java
deleted file mode 100644
index c1646e1..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/OI.java
+++ /dev/null
@@ -1,60 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat;
-
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.buttons.JoystickButton;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.Collect;
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveForward;
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.LowGoal;
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.SetCollectionSpeed;
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.SetPivotSetpoint;
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.Shoot;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pivot;
-import edu.wpi.first.wpilibj.examples.pacgoat.triggers.DoubleButton;
-
-/**
- * The operator interface of the robot, it has been simplified from the real
- * robot to allow control with a single PS3 joystick. As a result, not all
- * functionality from the real robot is available.
- */
-public class OI {
- public Joystick m_joystick = new Joystick(0);
-
- /**
- * Create a new OI and all of the buttons on it.
- */
- public OI() {
- new JoystickButton(m_joystick, 12).whenPressed(new LowGoal());
- new JoystickButton(m_joystick, 10).whenPressed(new Collect());
-
- new JoystickButton(m_joystick, 11).whenPressed(
- new SetPivotSetpoint(Pivot.kShoot));
- new JoystickButton(m_joystick, 9).whenPressed(
- new SetPivotSetpoint(Pivot.kShootNear));
-
- new DoubleButton(m_joystick, 2, 3).whenActive(new Shoot());
-
- // SmartDashboard Buttons
- SmartDashboard.putData("Drive Forward", new DriveForward(2.25));
- SmartDashboard.putData("Drive Backward", new DriveForward(-2.25));
- SmartDashboard.putData("Start Rollers",
- new SetCollectionSpeed(Collector.kForward));
- SmartDashboard.putData("Stop Rollers",
- new SetCollectionSpeed(Collector.kStop));
- SmartDashboard.putData("Reverse Rollers",
- new SetCollectionSpeed(Collector.kReverse));
- }
-
- public Joystick getJoystick() {
- return m_joystick;
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java
deleted file mode 100644
index 0f4da55..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java
+++ /dev/null
@@ -1,128 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat;
-
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.command.Scheduler;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveAndShootAutonomous;
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveForward;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.DriveTrain;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pivot;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pneumatics;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Shooter;
-
-/**
- * This is the main class for running the PacGoat code.
- *
- * <p>The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the TimedRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the manifest file in the resource
- * directory.
- */
-public class Robot extends TimedRobot {
- Command m_autonomousCommand;
- public static OI oi;
-
- // Initialize the subsystems
- public static DriveTrain drivetrain = new DriveTrain();
- public static Collector collector = new Collector();
- public static Shooter shooter = new Shooter();
- public static Pneumatics pneumatics = new Pneumatics();
- public static Pivot pivot = new Pivot();
-
- public SendableChooser<Command> m_autoChooser;
-
- // This function is run when the robot is first started up and should be
- // used for any initialization code.
- @Override
- public void robotInit() {
- SmartDashboard.putData(drivetrain);
- SmartDashboard.putData(collector);
- SmartDashboard.putData(shooter);
- SmartDashboard.putData(pneumatics);
- SmartDashboard.putData(pivot);
-
- // This MUST be here. If the OI creates Commands (which it very likely
- // will), constructing it during the construction of CommandBase (from
- // which commands extend), subsystems are not guaranteed to be
- // yet. Thus, their requires() statements may grab null pointers. Bad
- // news. Don't move it.
- oi = new OI();
-
- // instantiate the command used for the autonomous period
- m_autoChooser = new SendableChooser<>();
- m_autoChooser.setDefaultOption("Drive and Shoot", new DriveAndShootAutonomous());
- m_autoChooser.addOption("Drive Forward", new DriveForward());
- SmartDashboard.putData("Auto Mode", m_autoChooser);
- }
-
- @Override
- public void autonomousInit() {
- m_autonomousCommand = m_autoChooser.getSelected();
- m_autonomousCommand.start();
- }
-
- // This function is called periodically during autonomous
- @Override
- public void autonomousPeriodic() {
- Scheduler.getInstance().run();
- log();
- }
-
- @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() {
- Scheduler.getInstance().run();
- log();
- }
-
- // This function called periodically during test mode
- @Override
- public void testPeriodic() {
- }
-
- @Override
- public void disabledInit() {
- Robot.shooter.unlatch();
- }
-
- // This function is called periodically while disabled
- @Override
- public void disabledPeriodic() {
- log();
- }
-
- /**
- * Log interesting values to the SmartDashboard.
- */
- private void log() {
- Robot.pneumatics.writePressure();
- SmartDashboard.putNumber("Pivot Pot Value", Robot.pivot.getAngle());
- SmartDashboard.putNumber("Left Distance",
- drivetrain.getLeftEncoder().getDistance());
- SmartDashboard.putNumber("Right Distance",
- drivetrain.getRightEncoder().getDistance());
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CheckForHotGoal.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CheckForHotGoal.java
deleted file mode 100644
index 4136ed1..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CheckForHotGoal.java
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * This command looks for the hot goal and waits until it's detected or timed
- * out. The timeout is because it's better to shoot and get some autonomous
- * points than get none. When called sequentially, this command will block until
- * the hot goal is detected or until it is timed out.
- */
-public class CheckForHotGoal extends Command {
- public CheckForHotGoal(double time) {
- setTimeout(time);
- }
-
- // Make this return true when this Command no longer needs to run execute()
- @Override
- protected boolean isFinished() {
- return isTimedOut() || Robot.shooter.goalIsHot();
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CloseClaw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CloseClaw.java
deleted file mode 100644
index e9c154a..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CloseClaw.java
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.InstantCommand;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * Close the claw.
- *
- * <p>NOTE: It doesn't wait for the claw to close since there is no sensor to
- * detect that.
- */
-public class CloseClaw extends InstantCommand {
- public CloseClaw() {
- requires(Robot.collector);
- }
-
- // Called just before this Command runs the first time
- @Override
- protected void initialize() {
- Robot.collector.close();
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Collect.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Collect.java
deleted file mode 100644
index d9db916..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Collect.java
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pivot;
-
-/**
- * Get the robot set to collect balls.
- */
-public class Collect extends CommandGroup {
- /**
- * Create a new collect command.
- */
- public Collect() {
- addSequential(new SetCollectionSpeed(Collector.kForward));
- addParallel(new CloseClaw());
- addSequential(new SetPivotSetpoint(Pivot.kCollect));
- addSequential(new WaitForBall());
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveAndShootAutonomous.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveAndShootAutonomous.java
deleted file mode 100644
index f614e8f..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveAndShootAutonomous.java
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * Drive over the line and then shoot the ball. If the hot goal is not detected,
- * it will wait briefly.
- */
-public class DriveAndShootAutonomous extends CommandGroup {
- /**
- * Create a new drive and shoot autonomous.
- */
- public DriveAndShootAutonomous() {
- addSequential(new CloseClaw());
- addSequential(new WaitForPressure(), 2);
- if (Robot.isReal()) {
- // NOTE: Simulation doesn't currently have the concept of hot.
- addSequential(new CheckForHotGoal(2));
- }
- addSequential(new SetPivotSetpoint(45));
- addSequential(new DriveForward(8, 0.3));
- addSequential(new Shoot());
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveForward.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveForward.java
deleted file mode 100644
index 219233b..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveForward.java
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * This command drives the robot over a given distance with simple proportional
- * control This command will drive a given distance limiting to a maximum speed.
- */
-public class DriveForward extends Command {
- private final double m_driveForwardSpeed;
- private final double m_distance;
- private double m_error;
- private static final double kTolerance = 0.1;
- private static final double kP = -1.0 / 5.0;
-
- public DriveForward() {
- this(10, 0.5);
- }
-
- public DriveForward(double dist) {
- this(dist, 0.5);
- }
-
- /**
- * Create a new drive forward command.
- * @param dist The distance to drive
- * @param maxSpeed The maximum speed to drive at
- */
- public DriveForward(double dist, double maxSpeed) {
- requires(Robot.drivetrain);
- m_distance = dist;
- m_driveForwardSpeed = maxSpeed;
- }
-
- @Override
- protected void initialize() {
- Robot.drivetrain.getRightEncoder().reset();
- setTimeout(2);
- }
-
- @Override
- protected void execute() {
- m_error = m_distance - Robot.drivetrain.getRightEncoder().getDistance();
- if (m_driveForwardSpeed * kP * m_error >= m_driveForwardSpeed) {
- Robot.drivetrain.tankDrive(m_driveForwardSpeed, m_driveForwardSpeed);
- } else {
- Robot.drivetrain.tankDrive(m_driveForwardSpeed * kP * m_error,
- m_driveForwardSpeed * kP * m_error);
- }
- }
-
- @Override
- protected boolean isFinished() {
- return Math.abs(m_error) <= kTolerance || isTimedOut();
- }
-
- @Override
- protected void end() {
- Robot.drivetrain.stop();
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveWithJoystick.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveWithJoystick.java
deleted file mode 100644
index 883ff62..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveWithJoystick.java
+++ /dev/null
@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * This command allows PS3 joystick to drive the robot. It is always running
- * except when interrupted by another command.
- */
-public class DriveWithJoystick extends Command {
- public DriveWithJoystick() {
- requires(Robot.drivetrain);
- }
-
- @Override
- protected void execute() {
- Robot.drivetrain.tankDrive(Robot.oi.getJoystick());
- }
-
- @Override
- protected boolean isFinished() {
- return false;
- }
-
- @Override
- protected void end() {
- Robot.drivetrain.stop();
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/ExtendShooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/ExtendShooter.java
deleted file mode 100644
index cfad93d..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/ExtendShooter.java
+++ /dev/null
@@ -1,34 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.TimedCommand;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * Extend the shooter and then retract it after a second.
- */
-public class ExtendShooter extends TimedCommand {
- public ExtendShooter() {
- super(1);
- requires(Robot.shooter);
- }
-
- // Called just before this Command runs the first time
- @Override
- protected void initialize() {
- Robot.shooter.extendBoth();
- }
-
- // Called once after isFinished returns true
- @Override
- protected void end() {
- Robot.shooter.retractBoth();
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/LowGoal.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/LowGoal.java
deleted file mode 100644
index 08214e1..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/LowGoal.java
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pivot;
-
-/**
- * Spit the ball out into the low goal assuming that the robot is in front of
- * it.
- */
-public class LowGoal extends CommandGroup {
- /**
- * Create a new low goal command.
- */
- public LowGoal() {
- addSequential(new SetPivotSetpoint(Pivot.kLowGoal));
- addSequential(new SetCollectionSpeed(Collector.kReverse));
- addSequential(new ExtendShooter());
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/OpenClaw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/OpenClaw.java
deleted file mode 100644
index f59a6f6..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/OpenClaw.java
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * Opens the claw.
- */
-public class OpenClaw extends Command {
- public OpenClaw() {
- requires(Robot.collector);
- }
-
- // Called just before this Command runs the first time
- @Override
- protected void initialize() {
- Robot.collector.open();
- }
-
- // Make this return true when this Command no longer needs to run execute()
- @Override
- protected boolean isFinished() {
- return Robot.collector.isOpen();
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetCollectionSpeed.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetCollectionSpeed.java
deleted file mode 100644
index 172905d..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetCollectionSpeed.java
+++ /dev/null
@@ -1,32 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.InstantCommand;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * This command sets the collector rollers spinning at the given speed. Since
- * there is no sensor for detecting speed, it finishes immediately. As a result,
- * the spinners may still be adjusting their speed.
- */
-public class SetCollectionSpeed extends InstantCommand {
- private final double m_speed;
-
- public SetCollectionSpeed(double speed) {
- requires(Robot.collector);
- this.m_speed = speed;
- }
-
- // Called just before this Command runs the first time
- @Override
- protected void initialize() {
- Robot.collector.setSpeed(m_speed);
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetPivotSetpoint.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetPivotSetpoint.java
deleted file mode 100644
index 74a81a0..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetPivotSetpoint.java
+++ /dev/null
@@ -1,39 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * Moves the pivot to a given angle. This command finishes when it is within the
- * tolerance, but leaves the PID loop running to maintain the position. Other
- * commands using the pivot should make sure they disable PID!
- */
-public class SetPivotSetpoint extends Command {
- private final double m_setpoint;
-
- public SetPivotSetpoint(double setpoint) {
- this.m_setpoint = setpoint;
- requires(Robot.pivot);
- }
-
- // Called just before this Command runs the first time
- @Override
- protected void initialize() {
- Robot.pivot.enable();
- Robot.pivot.setSetpoint(m_setpoint);
- }
-
- // Make this return true when this Command no longer needs to run execute()
- @Override
- protected boolean isFinished() {
- return Robot.pivot.onTarget();
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Shoot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Shoot.java
deleted file mode 100644
index 40ccb25..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Shoot.java
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
-
-/**
- * Shoot the ball at the current angle.
- */
-public class Shoot extends CommandGroup {
- /**
- * Create a new shoot command.
- */
- public Shoot() {
- addSequential(new WaitForPressure());
- addSequential(new SetCollectionSpeed(Collector.kStop));
- addSequential(new OpenClaw());
- addSequential(new ExtendShooter());
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForBall.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForBall.java
deleted file mode 100644
index bf1d506..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForBall.java
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * Wait until the collector senses that it has the ball. This command does
- * nothing and is intended to be used in command groups to wait for this
- * condition.
- */
-public class WaitForBall extends Command {
- public WaitForBall() {
- requires(Robot.collector);
- }
-
- // Make this return true when this Command no longer needs to run execute()
- @Override
- protected boolean isFinished() {
- return Robot.collector.hasBall();
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForPressure.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForPressure.java
deleted file mode 100644
index 39bac0a..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForPressure.java
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * Wait until the pneumatics are fully pressurized. This command does nothing
- * and is intended to be used in command groups to wait for this condition.
- */
-public class WaitForPressure extends Command {
- public WaitForPressure() {
- requires(Robot.pneumatics);
- }
-
- // Make this return true when this Command no longer needs to run execute()
- @Override
- protected boolean isFinished() {
- return Robot.pneumatics.isPressurized();
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java
deleted file mode 100644
index 853b51a..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java
+++ /dev/null
@@ -1,103 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.subsystems;
-
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.Solenoid;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.Victor;
-import edu.wpi.first.wpilibj.command.Subsystem;
-
-/**
- * The Collector subsystem has one motor for the rollers, a limit switch for
- * ball detection, a piston for opening and closing the claw, and a reed switch
- * to check if the piston is open.
- */
-public class Collector extends Subsystem implements AutoCloseable {
- // Constants for some useful speeds
- public static final double kForward = 1;
- public static final double kStop = 0;
- public static final double kReverse = -1;
-
- // Subsystem devices
- private final SpeedController m_rollerMotor = new Victor(6);
- private final DigitalInput m_ballDetector = new DigitalInput(10);
- private final DigitalInput m_openDetector = new DigitalInput(6);
- private final Solenoid m_piston = new Solenoid(1, 1);
-
- /**
- * Create a new collector subsystem.
- */
- public Collector() {
- // Put everything to the LiveWindow for testing.
- addChild("Roller Motor", (Victor) m_rollerMotor);
- addChild("Ball Detector", m_ballDetector);
- addChild("Claw Open Detector", m_openDetector);
- addChild("Piston", m_piston);
- }
-
- /**
- * Whether or not the robot has the ball.
- *
- * <p>NOTE: The current simulation model uses the the lower part of the claw
- * since the limit switch wasn't exported. At some point, this will be
- * updated.
- *
- * @return Whether or not the robot has the ball.
- */
- public boolean hasBall() {
- return m_ballDetector.get(); // TODO: prepend ! to reflect real robot
- }
-
- /**
- * Set the speed to spin the collector rollers.
- *
- * @param speed The speed to spin the rollers.
- */
- public void setSpeed(double speed) {
- m_rollerMotor.set(-speed);
- }
-
- /**
- * Stop the rollers from spinning.
- */
- public void stop() {
- m_rollerMotor.set(0);
- }
-
- /**
- * Wether or not the claw is open.
- *
- * @return Whether or not the claw is open.
- */
- public boolean isOpen() {
- return m_openDetector.get(); // TODO: prepend ! to reflect real robot
- }
-
- /**
- * Open the claw up (For shooting).
- */
- public void open() {
- m_piston.set(true);
- }
-
- /**
- * Close the claw (For collecting and driving).
- */
- @Override
- public void close() {
- m_piston.set(false);
- }
-
- /**
- * No default command.
- */
- @Override
- protected void initDefaultCommand() {
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java
deleted file mode 100644
index 9b165de..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java
+++ /dev/null
@@ -1,138 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.subsystems;
-
-import edu.wpi.first.wpilibj.AnalogGyro;
-import edu.wpi.first.wpilibj.CounterBase.EncodingType;
-import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
-import edu.wpi.first.wpilibj.Victor;
-import edu.wpi.first.wpilibj.command.Subsystem;
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveWithJoystick;
-
-/**
- * The DriveTrain subsystem controls the robot's chassis and reads in
- * information about it's speed and position.
- */
-public class DriveTrain extends Subsystem {
- // Subsystem devices
- private final SpeedController m_frontLeftCIM = new Victor(1);
- private final SpeedController m_frontRightCIM = new Victor(2);
- private final SpeedController m_rearLeftCIM = new Victor(3);
- private final SpeedController m_rearRightCIM = new Victor(4);
- private final SpeedControllerGroup m_leftCIMs = new SpeedControllerGroup(
- m_frontLeftCIM, m_rearLeftCIM);
- private final SpeedControllerGroup m_rightCIMs = new SpeedControllerGroup(
- m_frontRightCIM, m_rearRightCIM);
- private final DifferentialDrive m_drive;
- private final Encoder m_rightEncoder = new Encoder(1, 2, true, EncodingType.k4X);
- private final Encoder m_leftEncoder = new Encoder(3, 4, false, EncodingType.k4X);
- private final AnalogGyro m_gyro = new AnalogGyro(0);
-
- /**
- * Create a new drive train subsystem.
- */
- public DriveTrain() {
- // Configure drive motors
- addChild("Front Left CIM", (Victor) m_frontLeftCIM);
- addChild("Front Right CIM", (Victor) m_frontRightCIM);
- addChild("Back Left CIM", (Victor) m_rearLeftCIM);
- addChild("Back Right CIM", (Victor) m_rearRightCIM);
-
- // Configure the DifferentialDrive to reflect the fact that all motors
- // are wired backwards (right is inverted in DifferentialDrive).
- m_leftCIMs.setInverted(true);
- m_drive = new DifferentialDrive(m_leftCIMs, m_rightCIMs);
- m_drive.setSafetyEnabled(true);
- m_drive.setExpiration(0.1);
- m_drive.setMaxOutput(1.0);
-
- if (Robot.isReal()) { // Converts to feet
- m_rightEncoder.setDistancePerPulse(0.0785398);
- m_leftEncoder.setDistancePerPulse(0.0785398);
- } else {
- // Convert to feet 4in diameter wheels with 360 tick sim encoders
- m_rightEncoder.setDistancePerPulse(
- (4.0/* in */ * Math.PI) / (360.0 * 12.0/* in/ft */));
- m_leftEncoder.setDistancePerPulse(
- (4.0/* in */ * Math.PI) / (360.0 * 12.0/* in/ft */));
- }
-
- addChild("Right Encoder", m_rightEncoder);
- addChild("Left Encoder", m_leftEncoder);
-
- // Configure gyro
- if (Robot.isReal()) {
- m_gyro.setSensitivity(0.007); // TODO: Handle more gracefully?
- }
- addChild("Gyro", m_gyro);
- }
-
- /**
- * When other commands aren't using the drivetrain, allow tank drive with
- * the joystick.
- */
- @Override
- public void initDefaultCommand() {
- setDefaultCommand(new DriveWithJoystick());
- }
-
- /**
- * Tank drive using a PS3 joystick.
- *
- * @param joy PS3 style joystick to use as the input for tank drive.
- */
- public void tankDrive(Joystick joy) {
- m_drive.tankDrive(joy.getY(), joy.getRawAxis(4));
- }
-
- /**
- * Tank drive using individual joystick axes.
- *
- * @param leftAxis Left sides value
- * @param rightAxis Right sides value
- */
- public void tankDrive(double leftAxis, double rightAxis) {
- m_drive.tankDrive(leftAxis, rightAxis);
- }
-
- /**
- * Stop the drivetrain from moving.
- */
- public void stop() {
- m_drive.tankDrive(0, 0);
- }
-
- /**
- * The encoder getting the distance and speed of left side of the
- * drivetrain.
- */
- public Encoder getLeftEncoder() {
- return m_leftEncoder;
- }
-
- /**
- * The encoder getting the distance and speed of right side of the
- * drivetrain.
- */
- public Encoder getRightEncoder() {
- return m_rightEncoder;
- }
-
- /**
- * The current angle of the drivetrain as measured by the Gyro.
- */
- public double getAngle() {
- return m_gyro.getAngle();
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java
deleted file mode 100644
index 44e197c..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java
+++ /dev/null
@@ -1,107 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.subsystems;
-
-import edu.wpi.first.wpilibj.AnalogPotentiometer;
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.Victor;
-import edu.wpi.first.wpilibj.command.PIDSubsystem;
-import edu.wpi.first.wpilibj.interfaces.Potentiometer;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * The Pivot subsystem contains the Van-door motor and the pot for PID control
- * of angle of the pivot and claw.
- */
-public class Pivot extends PIDSubsystem {
- // Constants for some useful angles
- public static final double kCollect = 105;
- public static final double kLowGoal = 90;
- public static final double kShoot = 45;
- public static final double kShootNear = 30;
-
- // Sensors for measuring the position of the pivot.
- private final DigitalInput m_upperLimitSwitch = new DigitalInput(13);
- private final DigitalInput m_lowerLimitSwitch = new DigitalInput(12);
-
- // 0 degrees is vertical facing up.
- // Angle increases the more forward the pivot goes.
- private final Potentiometer m_pot = new AnalogPotentiometer(1);
-
- // Motor to move the pivot.
- private final SpeedController m_motor = new Victor(5);
-
- /**
- * Create a new pivot subsystem.
- */
- public Pivot() {
- super("Pivot", 7.0, 0.0, 8.0);
- setAbsoluteTolerance(0.005);
- getPIDController().setContinuous(false);
- if (Robot.isSimulation()) { // PID is different in simulation.
- getPIDController().setPID(0.5, 0.001, 2);
- setAbsoluteTolerance(5);
- }
-
- // Put everything to the LiveWindow for testing.
- addChild("Upper Limit Switch", m_upperLimitSwitch);
- addChild("Lower Limit Switch", m_lowerLimitSwitch);
- addChild("Pot", (AnalogPotentiometer) m_pot);
- addChild("Motor", (Victor) m_motor);
- addChild("PIDSubsystem Controller", getPIDController());
- }
-
- /**
- * No default command, if PID is enabled, the current setpoint will be
- * maintained.
- */
- @Override
- public void initDefaultCommand() {
- }
-
- /**
- * The angle read in by the potentiometer.
- */
- @Override
- protected double returnPIDInput() {
- return m_pot.get();
- }
-
- /**
- * Set the motor speed based off of the PID output.
- */
- @Override
- protected void usePIDOutput(double output) {
- m_motor.pidWrite(output);
- }
-
- /**
- * If the pivot is at its upper limit.
- */
- public boolean isAtUpperLimit() {
- // TODO: inverted from real robot (prefix with !)
- return m_upperLimitSwitch.get();
- }
-
- /**
- * If the pivot is at its lower limit.
- */
- public boolean isAtLowerLimit() {
- // TODO: inverted from real robot (prefix with !)
- return m_lowerLimitSwitch.get();
- }
-
- /**
- * The current angle of the pivot.
- */
- public double getAngle() {
- return m_pot.get();
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java
deleted file mode 100644
index 88dc838..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java
+++ /dev/null
@@ -1,55 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.subsystems;
-
-import edu.wpi.first.wpilibj.AnalogInput;
-import edu.wpi.first.wpilibj.command.Subsystem;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * The Pneumatics subsystem contains a pressure sensor.
- *
- * <p>NOTE: The simulator currently doesn't support the compressor or pressure
- * sensors.
- */
-public class Pneumatics extends Subsystem {
- AnalogInput m_pressureSensor = new AnalogInput(3);
-
- private static final double kMaxPressure = 2.55;
-
- public Pneumatics() {
- addChild("Pressure Sensor", m_pressureSensor);
- }
-
- /**
- * No default command.
- */
- @Override
- public void initDefaultCommand() {
- }
-
- /**
- * Whether or not the system is fully pressurized.
- */
- public boolean isPressurized() {
- if (Robot.isReal()) {
- return kMaxPressure <= m_pressureSensor.getVoltage();
- } else {
- return true; // NOTE: Simulation always has full pressure
- }
- }
-
- /**
- * Puts the pressure on the SmartDashboard.
- */
- public void writePressure() {
- SmartDashboard.putNumber("Pressure", m_pressureSensor.getVoltage());
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java
deleted file mode 100644
index 6fa7e68..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java
+++ /dev/null
@@ -1,176 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.subsystems;
-
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.DoubleSolenoid;
-import edu.wpi.first.wpilibj.Solenoid;
-import edu.wpi.first.wpilibj.command.Subsystem;
-
-/**
- * The Shooter subsystem handles shooting. The mechanism for shooting is
- * slightly complicated because it has to pneumatic cylinders for shooting, and
- * a third latch to allow the pressure to partially build up and reduce the
- * effect of the airflow. For shorter shots, when full power isn't needed, only
- * one cylinder fires.
- *
- * <p>NOTE: Simulation currently approximates this as as single pneumatic
- * cylinder and ignores the latch.
- */
-public class Shooter extends Subsystem {
- // Devices
- DoubleSolenoid m_piston1 = new DoubleSolenoid(1, 3, 4);
- DoubleSolenoid m_piston2 = new DoubleSolenoid(1, 5, 6);
- Solenoid m_latchPiston = new Solenoid(1, 2);
- DigitalInput m_piston1ReedSwitchFront = new DigitalInput(9);
- DigitalInput m_piston1ReedSwitchBack = new DigitalInput(11);
- //NOTE: currently ignored in simulation
- DigitalInput m_hotGoalSensor = new DigitalInput(7);
-
- /**
- * Create a new shooter subsystem.
- */
- public Shooter() {
- // Put everything to the LiveWindow for testing.
- addChild("Hot Goal Sensor", m_hotGoalSensor);
- addChild("Piston1 Reed Switch Front ", m_piston1ReedSwitchFront);
- addChild("Piston1 Reed Switch Back ", m_piston1ReedSwitchBack);
- addChild("Latch Piston", m_latchPiston);
- }
-
- /**
- * No default command.
- */
- @Override
- public void initDefaultCommand() {
- }
-
- /**
- * Extend both solenoids to shoot.
- */
- public void extendBoth() {
- m_piston1.set(DoubleSolenoid.Value.kForward);
- m_piston2.set(DoubleSolenoid.Value.kForward);
- }
-
- /**
- * Retract both solenoids to prepare to shoot.
- */
- public void retractBoth() {
- m_piston1.set(DoubleSolenoid.Value.kReverse);
- m_piston2.set(DoubleSolenoid.Value.kReverse);
- }
-
- /**
- * Extend solenoid 1 to shoot.
- */
- public void extend1() {
- m_piston1.set(DoubleSolenoid.Value.kForward);
- }
-
- /**
- * Retract solenoid 1 to prepare to shoot.
- */
- public void retract1() {
- m_piston1.set(DoubleSolenoid.Value.kReverse);
- }
-
- /**
- * Extend solenoid 2 to shoot.
- */
- public void extend2() {
- m_piston2.set(DoubleSolenoid.Value.kReverse);
- }
-
- /**
- * Retract solenoid 2 to prepare to shoot.
- */
- public void retract2() {
- m_piston2.set(DoubleSolenoid.Value.kForward);
- }
-
- /**
- * Turns off the piston1 double solenoid. This won't actuate anything
- * because double solenoids preserve their state when turned off. This
- * should be called in order to reduce the amount of time that the coils
- * are powered.
- */
- public void off1() {
- m_piston1.set(DoubleSolenoid.Value.kOff);
- }
-
- /**
- * Turns off the piston2 double solenoid. This won't actuate anything
- * because double solenoids preserve their state when turned off. This
- * should be called in order to reduce the amount of time that the coils
- * are powered.
- */
- public void off2() {
- m_piston2.set(DoubleSolenoid.Value.kOff);
- }
-
- /**
- * Release the latch so that we can shoot.
- */
- public void unlatch() {
- m_latchPiston.set(true);
- }
-
- /**
- * Latch so that pressure can build up and we aren't limited by air flow.
- */
- public void latch() {
- m_latchPiston.set(false);
- }
-
- /**
- * Toggles the latch postions.
- */
- public void toggleLatchPosition() {
- m_latchPiston.set(!m_latchPiston.get());
- }
-
- /**
- * Is Piston 1 extended (after shooting).
- *
- * @return Whether or not piston 1 is fully extended.
- */
- public boolean piston1IsExtended() {
- return !m_piston1ReedSwitchFront.get();
- }
-
- /**
- * Is Piston 1 retracted (before shooting).
- *
- * @return Whether or not piston 1 is fully retracted.
- */
- public boolean piston1IsRetracted() {
- return !m_piston1ReedSwitchBack.get();
- }
-
- /**
- * Turns off all double solenoids. Double solenoids hold their position when
- * they are turned off. We should turn them off whenever possible to extend
- * the life of the coils.
- */
- public void offBoth() {
- m_piston1.set(DoubleSolenoid.Value.kOff);
- m_piston2.set(DoubleSolenoid.Value.kOff);
- }
-
- /**
- * Return whether the goal is hot as read by the banner sensor.
- *
- * <p>NOTE: doesn't work in simulation.
- *
- * @return Whether or not the goal is hot
- */
- public boolean goalIsHot() {
- return m_hotGoalSensor.get();
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java
deleted file mode 100644
index 4fc8055..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java
+++ /dev/null
@@ -1,38 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.triggers;
-
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.buttons.Trigger;
-
-/**
- * A custom button that is triggered when TWO buttons on a Joystick are
- * simultaneously pressed.
- */
-public class DoubleButton extends Trigger {
- private final Joystick m_joy;
- private final int m_button1;
- private final int m_button2;
-
- /**
- * Create a new double button trigger.
- * @param joy The joystick
- * @param button1 The first button
- * @param button2 The second button
- */
- public DoubleButton(Joystick joy, int button1, int button2) {
- this.m_joy = joy;
- this.m_button1 = button1;
- this.m_button2 = button2;
- }
-
- @Override
- public boolean get() {
- return m_joy.getRawButton(m_button1) && m_joy.getRawButton(m_button2);
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Main.java
index 72b9454..21bcb5f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.potentiometerpid;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java
index cef0916..a1bb38f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java
@@ -1,23 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.potentiometerpid;
+import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
- * This is a sample program to demonstrate how to use a soft potentiometer and a
- * PID controller to reach and maintain position setpoints on an elevator
- * mechanism.
+ * This is a sample program to demonstrate how to use a soft potentiometer and a PID controller to
+ * reach and maintain position setpoints on an elevator mechanism.
*/
public class Robot extends TimedRobot {
private static final int kPotChannel = 1;
@@ -37,7 +33,7 @@
private PIDController m_pidController;
private AnalogInput m_potentiometer;
- private SpeedController m_elevatorMotor;
+ private MotorController m_elevatorMotor;
private Joystick m_joystick;
private int m_index;
@@ -46,17 +42,17 @@
@Override
public void robotInit() {
m_potentiometer = new AnalogInput(kPotChannel);
- m_elevatorMotor = new PWMVictorSPX(kMotorChannel);
+ m_elevatorMotor = new PWMSparkMax(kMotorChannel);
m_joystick = new Joystick(kJoystickChannel);
m_pidController = new PIDController(kP, kI, kD);
+ m_pidController.setSetpoint(kSetPoints[m_index]);
}
@Override
public void teleopPeriodic() {
// Run the PID Controller
- double pidOut
- = m_pidController.calculate(m_potentiometer.getAverageVoltage());
+ double pidOut = m_pidController.calculate(m_potentiometer.getAverageVoltage());
m_elevatorMotor.set(pidOut);
// when the button is pressed once, the selected elevator setpoint
@@ -65,9 +61,8 @@
if (currentButtonValue && !m_previousButtonValue) {
// index of the elevator setpoint wraps around.
m_index = (m_index + 1) % kSetPoints.length;
+ m_pidController.setSetpoint(kSetPoints[m_index]);
}
m_previousButtonValue = currentButtonValue;
-
- m_pidController.setSetpoint(kSetPoints[m_index]);
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Main.java
index be7edc6..f3925a0 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.quickvision;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Robot.java
index 9816f13..a025c56 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.quickvision;
@@ -11,14 +8,13 @@
import edu.wpi.first.wpilibj.TimedRobot;
/**
- * Uses the CameraServer class to automatically capture video from a USB webcam
- * and send it to the FRC dashboard without doing any vision processing. This
- * is the easiest way to get camera images to the dashboard. Just add this to
- * the robotInit() method in your program.
+ * Uses the CameraServer class to automatically capture video from a USB webcam and send it to the
+ * FRC dashboard without doing any vision processing. This is the easiest way to get camera images
+ * to the dashboard. Just add this to the robotInit() method in your program.
*/
public class Robot extends TimedRobot {
@Override
public void robotInit() {
- CameraServer.getInstance().startAutomaticCapture();
+ CameraServer.startAutomaticCapture();
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
index 49044a2..b63766c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.ramsetecommand;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
/**
* 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.
+ * 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.
@@ -24,8 +21,8 @@
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
- public static final int[] kLeftEncoderPorts = new int[]{0, 1};
- public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+ public static final int[] kRightEncoderPorts = new int[] {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
@@ -53,7 +50,7 @@
}
public static final class OIConstants {
- public static final int kDriverControllerPort = 1;
+ public static final int kDriverControllerPort = 0;
}
public static final class AutoConstants {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java
index 4cce085..462edd4 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.ramsetecommand;
@@ -15,8 +12,7 @@
* call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java
index 18dc814..49f1975 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.ramsetecommand;
@@ -37,8 +34,8 @@
* 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.
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
@@ -49,20 +46,14 @@
CommandScheduler.getInstance().run();
}
- /**
- * This function is called once each time the robot enters Disabled mode.
- */
+ /** This function is called once each time the robot enters Disabled mode. */
@Override
- public void disabledInit() {
- }
+ public void disabledInit() {}
@Override
- public void disabledPeriodic() {
- }
+ public void disabledPeriodic() {}
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
}
}
- /**
- * This function is called periodically during autonomous.
- */
+ /** This function is called periodically during autonomous. */
@Override
- public void autonomousPeriodic() {
- }
+ public void autonomousPeriodic() {}
@Override
public void teleopInit() {
@@ -98,13 +86,9 @@
}
}
- /**
- * This function is called periodically during operator control.
- */
+ /** This function is called periodically during operator control. */
@Override
- public void teleopPeriodic() {
-
- }
+ public void teleopPeriodic() {}
@Override
public void testInit() {
@@ -112,10 +96,7 @@
CommandScheduler.getInstance().cancelAll();
}
- /**
- * This function is called periodically during test mode.
- */
+ /** This function is called periodically during test mode. */
@Override
- public void testPeriodic() {
- }
+ public void testPeriodic() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
index bb8dd28..680af28 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
@@ -1,43 +1,37 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.ramsetecommand;
-import java.util.List;
+import static edu.wpi.first.wpilibj.XboxController.Button;
-import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.RamseteController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+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.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.RamseteController;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.RamseteCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
import edu.wpi.first.wpilibj.examples.ramsetecommand.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 static edu.wpi.first.wpilibj.XboxController.Button;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import java.util.List;
/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * 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.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems
@@ -46,9 +40,7 @@
// The driver's controller
XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
- /**
- * The container for the robot. Contains subsystems, OI devices, and commands.
- */
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
@@ -58,82 +50,80 @@
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
- new RunCommand(() -> m_robotDrive
- .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
- m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
-
+ new RunCommand(
+ () ->
+ m_robotDrive.arcadeDrive(
+ m_driverController.getLeftY(), m_driverController.getRightX()),
+ m_robotDrive));
}
/**
- * Use this method to define your button->command mappings. Buttons can be created by
- * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
* {@link JoystickButton}.
*/
private void configureButtonBindings() {
// Drive at half speed when the right bumper is held
- new JoystickButton(m_driverController, Button.kBumperRight.value)
+ new JoystickButton(m_driverController, Button.kRightBumper.value)
.whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
-
}
-
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
-
// Create a voltage constraint to ensure we don't accelerate too fast
var autoVoltageConstraint =
new DifferentialDriveVoltageConstraint(
- new SimpleMotorFeedforward(DriveConstants.ksVolts,
- DriveConstants.kvVoltSecondsPerMeter,
- DriveConstants.kaVoltSecondsSquaredPerMeter),
+ new SimpleMotorFeedforward(
+ DriveConstants.ksVolts,
+ DriveConstants.kvVoltSecondsPerMeter,
+ DriveConstants.kaVoltSecondsSquaredPerMeter),
DriveConstants.kDriveKinematics,
10);
// Create config for trajectory
TrajectoryConfig config =
- new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
- AutoConstants.kMaxAccelerationMetersPerSecondSquared)
+ new TrajectoryConfig(
+ AutoConstants.kMaxSpeedMetersPerSecond,
+ AutoConstants.kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(DriveConstants.kDriveKinematics)
// Apply the voltage constraint
.addConstraint(autoVoltageConstraint);
// An example trajectory to follow. All units in meters.
- Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
- // Start at the origin facing the +X direction
- new Pose2d(0, 0, new Rotation2d(0)),
- // Pass through these two interior waypoints, making an 's' curve path
- List.of(
- new Translation2d(1, 1),
- new Translation2d(2, -1)
- ),
- // End 3 meters straight ahead of where we started, facing forward
- new Pose2d(3, 0, new Rotation2d(0)),
- // Pass config
- config
- );
+ Trajectory exampleTrajectory =
+ TrajectoryGenerator.generateTrajectory(
+ // Start at the origin facing the +X direction
+ new Pose2d(0, 0, new Rotation2d(0)),
+ // Pass through these two interior waypoints, making an 's' curve path
+ List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
+ // End 3 meters straight ahead of where we started, facing forward
+ new Pose2d(3, 0, new Rotation2d(0)),
+ // Pass config
+ config);
- RamseteCommand ramseteCommand = new RamseteCommand(
- exampleTrajectory,
- m_robotDrive::getPose,
- new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
- new SimpleMotorFeedforward(DriveConstants.ksVolts,
- DriveConstants.kvVoltSecondsPerMeter,
- DriveConstants.kaVoltSecondsSquaredPerMeter),
- DriveConstants.kDriveKinematics,
- m_robotDrive::getWheelSpeeds,
- new PIDController(DriveConstants.kPDriveVel, 0, 0),
- new PIDController(DriveConstants.kPDriveVel, 0, 0),
- // RamseteCommand passes volts to the callback
- m_robotDrive::tankDriveVolts,
- m_robotDrive
- );
+ RamseteCommand ramseteCommand =
+ new RamseteCommand(
+ exampleTrajectory,
+ m_robotDrive::getPose,
+ new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
+ new SimpleMotorFeedforward(
+ DriveConstants.ksVolts,
+ DriveConstants.kvVoltSecondsPerMeter,
+ DriveConstants.kaVoltSecondsSquaredPerMeter),
+ DriveConstants.kDriveKinematics,
+ m_robotDrive::getWheelSpeeds,
+ new PIDController(DriveConstants.kPDriveVel, 0, 0),
+ new PIDController(DriveConstants.kPDriveVel, 0, 0),
+ // RamseteCommand passes volts to the callback
+ m_robotDrive::tankDriveVolts,
+ m_robotDrive);
// Reset odometry to the starting pose of the trajectory.
m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
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 396d5df..8ca929d 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
@@ -1,48 +1,50 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.ramsetecommand.subsystems;
+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.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+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 SpeedControllerGroup m_leftMotors =
- new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
- new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
+ private final MotorControllerGroup m_leftMotors =
+ new MotorControllerGroup(
+ new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+ new PWMSparkMax(DriveConstants.kLeftMotor2Port));
// The motors on the right side of the drive.
- private final SpeedControllerGroup m_rightMotors =
- new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
- new PWMVictorSPX(DriveConstants.kRightMotor2Port));
+ private final MotorControllerGroup m_rightMotors =
+ new MotorControllerGroup(
+ new PWMSparkMax(DriveConstants.kRightMotor1Port),
+ new PWMSparkMax(DriveConstants.kRightMotor2Port));
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
// The left-side drive encoder
private final Encoder m_leftEncoder =
- new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
- DriveConstants.kLeftEncoderReversed);
+ 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);
+ new Encoder(
+ DriveConstants.kRightEncoderPorts[0],
+ DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
// The gyro sensor
private final Gyro m_gyro = new ADXRS450_Gyro();
@@ -50,10 +52,13 @@
// Odometry class for tracking robot pose
private final DifferentialDriveOdometry m_odometry;
- /**
- * Creates a new DriveSubsystem.
- */
+ /** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ // 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);
+
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -65,8 +70,8 @@
@Override
public void periodic() {
// Update the odometry in the periodic block
- m_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(),
- m_rightEncoder.getDistance());
+ m_odometry.update(
+ m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
/**
@@ -110,7 +115,7 @@
/**
* Controls the left and right sides of the drive directly with voltages.
*
- * @param leftVolts the commanded left output
+ * @param leftVolts the commanded left output
* @param rightVolts the commanded right output
*/
public void tankDriveVolts(double leftVolts, double rightVolts) {
@@ -119,9 +124,7 @@
m_drive.feed();
}
- /**
- * Resets the drive encoders to currently read a position of 0.
- */
+ /** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
@@ -155,7 +158,7 @@
}
/**
- * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
@@ -163,9 +166,7 @@
m_drive.setMaxOutput(maxOutput);
}
- /**
- * Zeroes the heading of the robot.
- */
+ /** Zeroes the heading of the robot. */
public void zeroHeading() {
m_gyro.reset();
}
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 388b32d..e119080 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
@@ -1,28 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.ramsetecontroller;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+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.
- */
+/** Represents a differential drive style drivetrain. */
public class Drivetrain {
public static final double kMaxSpeed = 3.0; // meters per second
public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
@@ -31,26 +26,26 @@
private static final double kWheelRadius = 0.0508; // meters
private static final int kEncoderResolution = 4096;
- private final SpeedController m_leftLeader = new PWMVictorSPX(1);
- private final SpeedController m_leftFollower = new PWMVictorSPX(2);
- private final SpeedController m_rightLeader = new PWMVictorSPX(3);
- private final SpeedController m_rightFollower = new PWMVictorSPX(4);
+ 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 Encoder m_leftEncoder = new Encoder(0, 1);
private final Encoder m_rightEncoder = new Encoder(2, 3);
- private final SpeedControllerGroup m_leftGroup
- = new SpeedControllerGroup(m_leftLeader, m_leftFollower);
- private final SpeedControllerGroup m_rightGroup
- = new SpeedControllerGroup(m_rightLeader, m_rightFollower);
+ 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);
private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
- private final DifferentialDriveKinematics m_kinematics
- = new DifferentialDriveKinematics(kTrackWidth);
+ private final DifferentialDriveKinematics m_kinematics =
+ new DifferentialDriveKinematics(kTrackWidth);
private final DifferentialDriveOdometry m_odometry;
@@ -58,12 +53,17 @@
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
/**
- * Constructs a differential drive object.
- * Sets the encoder distance per pulse and resets the gyro.
+ * Constructs a differential drive object. Sets the encoder distance per pulse and resets the
+ * gyro.
*/
public Drivetrain() {
m_gyro.reset();
+ // 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);
+
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
// resolution.
@@ -85,10 +85,10 @@
final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
- final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(),
- speeds.leftMetersPerSecond);
- final double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(),
- speeds.rightMetersPerSecond);
+ final double leftOutput =
+ 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);
}
@@ -97,7 +97,7 @@
* Drives the robot with the given linear velocity and angular velocity.
*
* @param xSpeed Linear velocity in m/s.
- * @param rot Angular velocity in rad/s.
+ * @param rot Angular velocity in rad/s.
*/
@SuppressWarnings("ParameterName")
public void drive(double xSpeed, double rot) {
@@ -105,16 +105,15 @@
setSpeeds(wheelSpeeds);
}
- /**
- * Updates the field-relative position.
- */
+ /** Updates the field-relative position. */
public void updateOdometry() {
- m_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(),
- m_rightEncoder.getDistance());
+ m_odometry.update(
+ m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
/**
* Resets the field-relative position to a specific location.
+ *
* @param pose The position to reset to.
*/
public void resetOdometry(Pose2d pose) {
@@ -123,6 +122,7 @@
/**
* Returns the pose of the robot.
+ *
* @return The pose of the robot.
*/
public Pose2d getPose() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java
index bd07c2e..0b59d82 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.ramsetecontroller;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java
index 6a95a24..ef73d10 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java
@@ -1,27 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.ramsetecontroller;
-import java.util.List;
-
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.SlewRateLimiter;
+import edu.wpi.first.math.controller.RamseteController;
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.controller.RamseteController;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.List;
public class Robot extends TimedRobot {
private final XboxController m_controller = new XboxController(0);
@@ -35,21 +32,31 @@
private Trajectory m_trajectory;
// The Ramsete Controller to follow the trajectory.
- private RamseteController m_ramseteController;
+ private final RamseteController m_ramseteController = new RamseteController();
// The timer to use during the autonomous period.
private Timer m_timer;
+ // Create Field2d for robot and trajectory visualizations.
+ private Field2d m_field;
+
@Override
public void robotInit() {
// Create the trajectory to follow in autonomous. It is best to initialize
// trajectories here to avoid wasting time in autonomous.
- m_trajectory = TrajectoryGenerator.generateTrajectory(
- new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
- List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
- new Pose2d(3, 0, Rotation2d.fromDegrees(0)),
- new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0))
- );
+ m_trajectory =
+ TrajectoryGenerator.generateTrajectory(
+ new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+ List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
+ new Pose2d(3, 0, Rotation2d.fromDegrees(0)),
+ new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0)));
+
+ // Create and push Field2d to SmartDashboard.
+ m_field = new Field2d();
+ SmartDashboard.putData(m_field);
+
+ // Push the trajectory to Field2d.
+ m_field.getObject("traj").setTrajectory(m_trajectory);
}
@Override
@@ -67,6 +74,9 @@
// Update odometry.
m_drive.updateOdometry();
+ // Update robot position on Field2d.
+ m_field.setRobotPose(m_drive.getPose());
+
if (m_timer.get() < m_trajectory.getTotalTimeSeconds()) {
// Get the desired pose from the trajectory.
var desiredPose = m_trajectory.sample(m_timer.get());
@@ -85,17 +95,13 @@
public void teleopPeriodic() {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
- final var xSpeed =
- -m_speedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
- * Drivetrain.kMaxSpeed;
+ final var xSpeed = -m_speedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
- final var rot =
- -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
- * Drivetrain.kMaxAngularSpeed;
+ final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
m_drive.drive(xSpeed, rot);
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Main.java
index 5cd6a00..c005ec7 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.relay;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Robot.java
index 18da837..39e505e 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.relay;
@@ -18,7 +15,6 @@
* compressor. This program uses two buttons on a joystick and each button corresponds to one
* output; pressing the button sets the output to 12V and releasing sets it to 0V.
*/
-
public class Robot extends TimedRobot {
private final Joystick m_stick = new Joystick(0);
private final Relay m_relay = new Relay(0);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Constants.java
new file mode 100644
index 0000000..2ebfb92
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Constants.java
@@ -0,0 +1,15 @@
+// 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.romireference;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Main.java
new file mode 100644
index 0000000..aa21a58
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/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.romireference;
+
+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/romireference/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Robot.java
new file mode 100644
index 0000000..0cd83a9
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Robot.java
@@ -0,0 +1,95 @@
+// 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.romireference;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /** This function is called once each time the robot enters Disabled mode. */
+ @Override
+ public void disabledInit() {}
+
+ @Override
+ public void disabledPeriodic() {}
+
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+ @Override
+ public void autonomousInit() {
+ // Get selected routine from the SmartDashboard
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /** This function is called periodically during autonomous. */
+ @Override
+ public void autonomousPeriodic() {}
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running which will
+ // use the default command which is ArcadeDrive. 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/romireference/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/RobotContainer.java
new file mode 100644
index 0000000..8ad7bc3
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/RobotContainer.java
@@ -0,0 +1,97 @@
+// 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.romireference;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.examples.romireference.commands.ArcadeDrive;
+import edu.wpi.first.wpilibj.examples.romireference.commands.AutonomousDistance;
+import edu.wpi.first.wpilibj.examples.romireference.commands.AutonomousTime;
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.OnBoardIO;
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.OnBoardIO.ChannelMode;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
+import edu.wpi.first.wpilibj2.command.button.Button;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems and commands are defined here...
+ private final Drivetrain m_drivetrain = new Drivetrain();
+ private final OnBoardIO m_onboardIO = new OnBoardIO(ChannelMode.INPUT, ChannelMode.INPUT);
+
+ // Assumes a gamepad plugged into channnel 0
+ private final Joystick m_controller = new Joystick(0);
+
+ // Create SmartDashboard chooser for autonomous routines
+ private final SendableChooser<Command> m_chooser = new SendableChooser<>();
+
+ // NOTE: The I/O pin functionality of the 5 exposed I/O pins depends on the hardware "overlay"
+ // that is specified when launching the wpilib-ws server on the Romi raspberry pi.
+ // By default, the following are available (listed in order from inside of the board to outside):
+ // - DIO 8 (mapped to Arduino pin 11, closest to the inside of the board)
+ // - Analog In 0 (mapped to Analog Channel 6 / Arduino Pin 4)
+ // - Analog In 1 (mapped to Analog Channel 2 / Arduino Pin 20)
+ // - PWM 2 (mapped to Arduino Pin 21)
+ // - PWM 3 (mapped to Arduino Pin 22)
+ //
+ // Your subsystem configuration should take the overlays into account
+
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+ * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ // Default command is arcade drive. This will run unless another command
+ // is scheduled over it.
+ m_drivetrain.setDefaultCommand(getArcadeDriveCommand());
+
+ // Example of how to use the onboard IO
+ Button onboardButtonA = new Button(m_onboardIO::getButtonAPressed);
+ onboardButtonA
+ .whenActive(new PrintCommand("Button A Pressed"))
+ .whenInactive(new PrintCommand("Button A Released"));
+
+ // Setup SmartDashboard options
+ m_chooser.setDefaultOption("Auto Routine Distance", new AutonomousDistance(m_drivetrain));
+ m_chooser.addOption("Auto Routine Time", new AutonomousTime(m_drivetrain));
+ SmartDashboard.putData(m_chooser);
+ }
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ return m_chooser.getSelected();
+ }
+
+ /**
+ * Use this to pass the teleop command to the main {@link Robot} class.
+ *
+ * @return the command to run in teleop
+ */
+ public Command getArcadeDriveCommand() {
+ return new ArcadeDrive(
+ m_drivetrain, () -> -m_controller.getRawAxis(1), () -> m_controller.getRawAxis(2));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/ArcadeDrive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/ArcadeDrive.java
new file mode 100644
index 0000000..4a82b38
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/ArcadeDrive.java
@@ -0,0 +1,53 @@
+// 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.romireference.commands;
+
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import java.util.function.Supplier;
+
+public class ArcadeDrive extends CommandBase {
+ private final Drivetrain m_drivetrain;
+ private final Supplier<Double> m_xaxisSpeedSupplier;
+ private final Supplier<Double> m_zaxisRotateSupplier;
+
+ /**
+ * Creates a new ArcadeDrive. This command will drive your robot according to the speed supplier
+ * lambdas. This command does not terminate.
+ *
+ * @param drivetrain The drivetrain subsystem on which this command will run
+ * @param xaxisSpeedSupplier Lambda supplier of forward/backward speed
+ * @param zaxisRotateSupplier Lambda supplier of rotational speed
+ */
+ public ArcadeDrive(
+ Drivetrain drivetrain,
+ Supplier<Double> xaxisSpeedSupplier,
+ Supplier<Double> zaxisRotateSupplier) {
+ m_drivetrain = drivetrain;
+ m_xaxisSpeedSupplier = xaxisSpeedSupplier;
+ m_zaxisRotateSupplier = zaxisRotateSupplier;
+ addRequirements(drivetrain);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {}
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ m_drivetrain.arcadeDrive(m_xaxisSpeedSupplier.get(), m_zaxisRotateSupplier.get());
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {}
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousDistance.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousDistance.java
new file mode 100644
index 0000000..d48b2b4
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousDistance.java
@@ -0,0 +1,24 @@
+// 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.romireference.commands;
+
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class AutonomousDistance extends SequentialCommandGroup {
+ /**
+ * Creates a new Autonomous Drive based on distance. This will drive out for a specified distance,
+ * turn around and drive back.
+ *
+ * @param drivetrain The drivetrain subsystem on which this command will run
+ */
+ public AutonomousDistance(Drivetrain drivetrain) {
+ addCommands(
+ new DriveDistance(-0.5, 10, drivetrain),
+ new TurnDegrees(-0.5, 180, drivetrain),
+ new DriveDistance(-0.5, 10, drivetrain),
+ new TurnDegrees(0.5, 180, drivetrain));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousTime.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousTime.java
new file mode 100644
index 0000000..4cc9afa
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousTime.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.romireference.commands;
+
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class AutonomousTime extends SequentialCommandGroup {
+ /**
+ * Creates a new Autonomous Drive based on time. This will drive out for a period of time, turn
+ * around for time (equivalent to time to turn around) and drive forward again. This should mimic
+ * driving out, turning around and driving back.
+ *
+ * @param drivetrain The drive subsystem on which this command will run
+ */
+ public AutonomousTime(Drivetrain drivetrain) {
+ addCommands(
+ new DriveTime(-0.6, 2.0, drivetrain),
+ new TurnTime(-0.5, 1.3, drivetrain),
+ new DriveTime(-0.6, 2.0, drivetrain),
+ new TurnTime(0.5, 1.3, drivetrain));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveDistance.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveDistance.java
new file mode 100644
index 0000000..f316ce6
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveDistance.java
@@ -0,0 +1,55 @@
+// 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.romireference.commands;
+
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class DriveDistance extends CommandBase {
+ private final Drivetrain m_drive;
+ private final double m_distance;
+ private final double m_speed;
+
+ /**
+ * Creates a new DriveDistance. This command will drive your your robot for a desired distance at
+ * a desired speed.
+ *
+ * @param speed The speed at which the robot will drive
+ * @param inches The number of inches the robot will drive
+ * @param drive The drivetrain subsystem on which this command will run
+ */
+ public DriveDistance(double speed, double inches, Drivetrain drive) {
+ m_distance = inches;
+ m_speed = speed;
+ m_drive = drive;
+ addRequirements(drive);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ m_drive.arcadeDrive(0, 0);
+ m_drive.resetEncoders();
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ m_drive.arcadeDrive(m_speed, 0);
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ m_drive.arcadeDrive(0, 0);
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ // Compare distance travelled from start to desired distance
+ return Math.abs(m_drive.getAverageDistanceInch()) >= m_distance;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveTime.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveTime.java
new file mode 100644
index 0000000..80636f0
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveTime.java
@@ -0,0 +1,54 @@
+// 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.romireference.commands;
+
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class DriveTime extends CommandBase {
+ private final double m_duration;
+ private final double m_speed;
+ private final Drivetrain m_drive;
+ private long m_startTime;
+
+ /**
+ * Creates a new DriveTime. This command will drive your robot for a desired speed and time.
+ *
+ * @param speed The speed which the robot will drive. Negative is in reverse.
+ * @param time How much time to drive in seconds
+ * @param drive The drivetrain subsystem on which this command will run
+ */
+ public DriveTime(double speed, double time, Drivetrain drive) {
+ m_speed = speed;
+ m_duration = time * 1000;
+ m_drive = drive;
+ addRequirements(drive);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ m_startTime = System.currentTimeMillis();
+ m_drive.arcadeDrive(0, 0);
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ m_drive.arcadeDrive(m_speed, 0);
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ m_drive.arcadeDrive(0, 0);
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return (System.currentTimeMillis() - m_startTime) >= m_duration;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnDegrees.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnDegrees.java
new file mode 100644
index 0000000..78ffa6c
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnDegrees.java
@@ -0,0 +1,68 @@
+// 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.romireference.commands;
+
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class TurnDegrees extends CommandBase {
+ private final Drivetrain m_drive;
+ private final double m_degrees;
+ private final double m_speed;
+
+ /**
+ * Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in
+ * degrees) and rotational speed.
+ *
+ * @param speed The speed which the robot will drive. Negative is in reverse.
+ * @param degrees Degrees to turn. Leverages encoders to compare distance.
+ * @param drive The drive subsystem on which this command will run
+ */
+ public TurnDegrees(double speed, double degrees, Drivetrain drive) {
+ m_degrees = degrees;
+ m_speed = speed;
+ m_drive = drive;
+ addRequirements(drive);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ // Set motors to stop, read encoder values for starting point
+ m_drive.arcadeDrive(0, 0);
+ m_drive.resetEncoders();
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ m_drive.arcadeDrive(0, m_speed);
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ m_drive.arcadeDrive(0, 0);
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ /* Need to convert distance travelled to degrees. The Standard
+ Romi Chassis found here, https://www.pololu.com/category/203/romi-chassis-kits,
+ has a wheel placement diameter (149 mm) - width of the wheel (8 mm) = 141 mm
+ or 5.551 inches. We then take into consideration the width of the tires.
+ */
+ double inchPerDegree = Math.PI * 5.551 / 360;
+ // Compare distance travelled from start to distance based on degree turn
+ return getAverageTurningDistance() >= (inchPerDegree * m_degrees);
+ }
+
+ private double getAverageTurningDistance() {
+ double leftDistance = Math.abs(m_drive.getLeftDistanceInch());
+ double rightDistance = Math.abs(m_drive.getRightDistanceInch());
+ return (leftDistance + rightDistance) / 2.0;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnTime.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnTime.java
new file mode 100644
index 0000000..10341f9
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnTime.java
@@ -0,0 +1,58 @@
+// 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.romireference.commands;
+
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/*
+ * Creates a new TurnTime command. This command will turn your robot for a
+ * desired rotational speed and time.
+ */
+public class TurnTime extends CommandBase {
+ private final double m_duration;
+ private final double m_rotationalSpeed;
+ private final Drivetrain m_drive;
+ private long m_startTime;
+
+ /**
+ * Creates a new TurnTime.
+ *
+ * @param speed The speed which the robot will turn. Negative is in reverse.
+ * @param time How much time to turn in seconds
+ * @param drive The drive subsystem on which this command will run
+ */
+ public TurnTime(double speed, double time, Drivetrain drive) {
+ m_rotationalSpeed = speed;
+ m_duration = time * 1000;
+ m_drive = drive;
+ addRequirements(drive);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ m_startTime = System.currentTimeMillis();
+ m_drive.arcadeDrive(0, 0);
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ m_drive.arcadeDrive(0, m_rotationalSpeed);
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ m_drive.arcadeDrive(0, 0);
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return (System.currentTimeMillis() - m_startTime) >= m_duration;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java
new file mode 100644
index 0000000..c21f623
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java
@@ -0,0 +1,124 @@
+// 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.romireference.sensors;
+
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.hal.SimDouble;
+
+public class RomiGyro {
+ private SimDouble m_simRateX;
+ private SimDouble m_simRateY;
+ private SimDouble m_simRateZ;
+ private SimDouble m_simAngleX;
+ private SimDouble m_simAngleY;
+ private SimDouble m_simAngleZ;
+
+ private double m_angleXOffset;
+ private double m_angleYOffset;
+ private double m_angleZOffset;
+
+ /** Create a new RomiGyro. */
+ public RomiGyro() {
+ SimDevice gyroSimDevice = SimDevice.create("Gyro:RomiGyro");
+ if (gyroSimDevice != null) {
+ gyroSimDevice.createBoolean("init", Direction.kOutput, true);
+ m_simRateX = gyroSimDevice.createDouble("rate_x", Direction.kInput, 0.0);
+ m_simRateY = gyroSimDevice.createDouble("rate_y", Direction.kInput, 0.0);
+ m_simRateZ = gyroSimDevice.createDouble("rate_z", Direction.kInput, 0.0);
+
+ m_simAngleX = gyroSimDevice.createDouble("angle_x", Direction.kInput, 0.0);
+ m_simAngleY = gyroSimDevice.createDouble("angle_y", Direction.kInput, 0.0);
+ m_simAngleZ = gyroSimDevice.createDouble("angle_z", Direction.kInput, 0.0);
+ }
+ }
+
+ /**
+ * Get the rate of turn in degrees-per-second around the X-axis.
+ *
+ * @return rate of turn in degrees-per-second
+ */
+ public double getRateX() {
+ if (m_simRateX != null) {
+ return m_simRateX.get();
+ }
+
+ return 0.0;
+ }
+
+ /**
+ * Get the rate of turn in degrees-per-second around the Y-axis.
+ *
+ * @return rate of turn in degrees-per-second
+ */
+ public double getRateY() {
+ if (m_simRateY != null) {
+ return m_simRateY.get();
+ }
+
+ return 0.0;
+ }
+
+ /**
+ * Get the rate of turn in degrees-per-second around the Z-axis.
+ *
+ * @return rate of turn in degrees-per-second
+ */
+ public double getRateZ() {
+ if (m_simRateZ != null) {
+ return m_simRateZ.get();
+ }
+
+ return 0.0;
+ }
+
+ /**
+ * Get the currently reported angle around the X-axis.
+ *
+ * @return current angle around X-axis in degrees
+ */
+ public double getAngleX() {
+ if (m_simAngleX != null) {
+ return m_simAngleX.get() - m_angleXOffset;
+ }
+
+ return 0.0;
+ }
+
+ /**
+ * Get the currently reported angle around the X-axis.
+ *
+ * @return current angle around Y-axis in degrees
+ */
+ public double getAngleY() {
+ if (m_simAngleY != null) {
+ return m_simAngleY.get() - m_angleYOffset;
+ }
+
+ return 0.0;
+ }
+
+ /**
+ * Get the currently reported angle around the Z-axis.
+ *
+ * @return current angle around Z-axis in degrees
+ */
+ public double getAngleZ() {
+ if (m_simAngleZ != null) {
+ return m_simAngleZ.get() - m_angleZOffset;
+ }
+
+ return 0.0;
+ }
+
+ /** Reset the gyro angles to 0. */
+ public void reset() {
+ if (m_simAngleX != null) {
+ m_angleXOffset = m_simAngleX.get();
+ m_angleYOffset = m_simAngleY.get();
+ m_angleZOffset = m_simAngleZ.get();
+ }
+ }
+}
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
new file mode 100644
index 0000000..a7c8432
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java
@@ -0,0 +1,142 @@
+// 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.romireference.subsystems;
+
+import edu.wpi.first.wpilibj.BuiltInAccelerometer;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.examples.romireference.sensors.RomiGyro;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Drivetrain extends SubsystemBase {
+ private static final double kCountsPerRevolution = 1440.0;
+ private static final double kWheelDiameterInch = 2.75591; // 70 mm
+
+ // The Romi has the left and right motors set to
+ // PWM channels 0 and 1 respectively
+ private final Spark m_leftMotor = new Spark(0);
+ private final Spark m_rightMotor = new Spark(1);
+
+ // The Romi has onboard encoders that are hardcoded
+ // to use DIO pins 4/5 and 6/7 for the left and right
+ private final Encoder m_leftEncoder = new Encoder(4, 5);
+ 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);
+
+ // Set up the RomiGyro
+ private final RomiGyro m_gyro = new RomiGyro();
+
+ // Set up the BuiltInAccelerometer
+ private final BuiltInAccelerometer m_accelerometer = new BuiltInAccelerometer();
+
+ /** Creates a new Drivetrain. */
+ public Drivetrain() {
+ // 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);
+
+ // Use inches as unit for encoder distances
+ m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+ m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+ resetEncoders();
+ }
+
+ public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
+ m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
+ }
+
+ public void resetEncoders() {
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+ }
+
+ public int getLeftEncoderCount() {
+ return m_leftEncoder.get();
+ }
+
+ public int getRightEncoderCount() {
+ return m_rightEncoder.get();
+ }
+
+ public double getLeftDistanceInch() {
+ return m_leftEncoder.getDistance();
+ }
+
+ public double getRightDistanceInch() {
+ return m_rightEncoder.getDistance();
+ }
+
+ public double getAverageDistanceInch() {
+ return (getLeftDistanceInch() + getRightDistanceInch()) / 2.0;
+ }
+
+ /**
+ * The acceleration in the X-axis.
+ *
+ * @return The acceleration of the Romi along the X-axis in Gs
+ */
+ public double getAccelX() {
+ return m_accelerometer.getX();
+ }
+
+ /**
+ * The acceleration in the Y-axis.
+ *
+ * @return The acceleration of the Romi along the Y-axis in Gs
+ */
+ public double getAccelY() {
+ return m_accelerometer.getY();
+ }
+
+ /**
+ * The acceleration in the Z-axis.
+ *
+ * @return The acceleration of the Romi along the Z-axis in Gs
+ */
+ public double getAccelZ() {
+ return m_accelerometer.getZ();
+ }
+
+ /**
+ * Current angle of the Romi around the X-axis.
+ *
+ * @return The current angle of the Romi in degrees
+ */
+ public double getGyroAngleX() {
+ return m_gyro.getAngleX();
+ }
+
+ /**
+ * Current angle of the Romi around the Y-axis.
+ *
+ * @return The current angle of the Romi in degrees
+ */
+ public double getGyroAngleY() {
+ return m_gyro.getAngleY();
+ }
+
+ /**
+ * Current angle of the Romi around the Z-axis.
+ *
+ * @return The current angle of the Romi in degrees
+ */
+ public double getGyroAngleZ() {
+ return m_gyro.getAngleZ();
+ }
+
+ /** Reset the gyro. */
+ public void resetGyro() {
+ m_gyro.reset();
+ }
+
+ @Override
+ public void periodic() {
+ // This method will be called once per scheduler run
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/OnBoardIO.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/OnBoardIO.java
new file mode 100644
index 0000000..848aff5
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/OnBoardIO.java
@@ -0,0 +1,128 @@
+// 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.romireference.subsystems;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DigitalOutput;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * This class represents the onboard IO of the Romi reference robot. This includes the pushbuttons
+ * and LEDs.
+ *
+ * <p>DIO 0 - Button A (input only) DIO 1 - Button B (input) or Green LED (output) DIO 2 - Button C
+ * (input) or Red LED (output) DIO 3 - Yellow LED (output only)
+ */
+public class OnBoardIO extends SubsystemBase {
+ private final DigitalInput m_buttonA = new DigitalInput(0);
+ private final DigitalOutput m_yellowLed = new DigitalOutput(3);
+
+ // DIO 1
+ private DigitalInput m_buttonB;
+ private DigitalOutput m_greenLed;
+
+ // DIO 2
+ private DigitalInput m_buttonC;
+ private DigitalOutput m_redLed;
+
+ private static final double MESSAGE_INTERVAL = 1.0;
+ private double m_nextMessageTime;
+
+ public enum ChannelMode {
+ INPUT,
+ OUTPUT
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param dio1 Mode for DIO 1 (input = Button B, output = green LED)
+ * @param dio2 Mode for DIO 2 (input = Button C, output = red LED)
+ */
+ public OnBoardIO(ChannelMode dio1, ChannelMode dio2) {
+ if (dio1 == ChannelMode.INPUT) {
+ m_buttonB = new DigitalInput(1);
+ } else {
+ m_greenLed = new DigitalOutput(1);
+ }
+
+ if (dio2 == ChannelMode.INPUT) {
+ m_buttonC = new DigitalInput(2);
+ } else {
+ m_redLed = new DigitalOutput(2);
+ }
+ }
+
+ /** Gets if the A button is pressed. */
+ public boolean getButtonAPressed() {
+ return m_buttonA.get();
+ }
+
+ /** Gets if the B button is pressed. */
+ public boolean getButtonBPressed() {
+ if (m_buttonB != null) {
+ return m_buttonB.get();
+ }
+
+ double currentTime = Timer.getFPGATimestamp();
+ if (currentTime > m_nextMessageTime) {
+ DriverStation.reportError("Button B was not configured", true);
+ m_nextMessageTime = currentTime + MESSAGE_INTERVAL;
+ }
+ return false;
+ }
+
+ /** Gets if the C button is pressed. */
+ public boolean getButtonCPressed() {
+ if (m_buttonC != null) {
+ return m_buttonC.get();
+ }
+
+ double currentTime = Timer.getFPGATimestamp();
+ if (currentTime > m_nextMessageTime) {
+ DriverStation.reportError("Button C was not configured", true);
+ m_nextMessageTime = currentTime + MESSAGE_INTERVAL;
+ }
+ return false;
+ }
+
+ /** Sets the green LED. */
+ public void setGreenLed(boolean value) {
+ if (m_greenLed != null) {
+ m_greenLed.set(value);
+ } else {
+ double currentTime = Timer.getFPGATimestamp();
+ if (currentTime > m_nextMessageTime) {
+ DriverStation.reportError("Green LED was not configured", true);
+ m_nextMessageTime = currentTime + MESSAGE_INTERVAL;
+ }
+ }
+ }
+
+ /** Sets the red LED. */
+ public void setRedLed(boolean value) {
+ if (m_redLed != null) {
+ m_redLed.set(value);
+ } else {
+ double currentTime = Timer.getFPGATimestamp();
+ if (currentTime > m_nextMessageTime) {
+ DriverStation.reportError("Red LED was not configured", true);
+ m_nextMessageTime = currentTime + MESSAGE_INTERVAL;
+ }
+ }
+ }
+
+ /** Sets the yellow LED. */
+ public void setYellowLed(boolean value) {
+ m_yellowLed.set(value);
+ }
+
+ @Override
+ public void periodic() {
+ // This method will be called once per scheduler run
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Constants.java
index 899d074..38217cd 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Constants.java
@@ -1,28 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.schedulereventlogging;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants. This class should not be used for any other purpose. All constants should be
- * declared globally (i.e. public static). Do not put anything functional in this class.
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
/**
- * Example of an inner class. One can "import static [...].Constants.OIConstants.*" to gain
- * access to the constants contained within without having to preface the names with the class,
- * greatly reducing the amount of text required.
+ * Example of an inner class. One can "import static [...].Constants.OIConstants.*" to gain access
+ * to the constants contained within without having to preface the names with the class, greatly
+ * reducing the amount of text required.
*/
public static final class OIConstants {
// Example: the port of the driver's controller
- public static final int kDriverControllerPort = 1;
+ public static final int kDriverControllerPort = 0;
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Main.java
index fda3a44..8e7a679 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.schedulereventlogging;
@@ -15,8 +12,7 @@
* call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Robot.java
index bfdf0d7..b61794c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.schedulereventlogging;
@@ -37,8 +34,8 @@
* 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.
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
@@ -49,20 +46,14 @@
CommandScheduler.getInstance().run();
}
- /**
- * This function is called once each time the robot enters Disabled mode.
- */
+ /** This function is called once each time the robot enters Disabled mode. */
@Override
- public void disabledInit() {
- }
+ public void disabledInit() {}
@Override
- public void disabledPeriodic() {
- }
+ public void disabledPeriodic() {}
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
}
}
- /**
- * This function is called periodically during autonomous.
- */
+ /** This function is called periodically during autonomous. */
@Override
- public void autonomousPeriodic() {
- }
+ public void autonomousPeriodic() {}
@Override
public void teleopInit() {
@@ -98,12 +86,9 @@
}
}
- /**
- * This function is called periodically during operator control.
- */
+ /** This function is called periodically during operator control. */
@Override
- public void teleopPeriodic() {
- }
+ public void teleopPeriodic() {}
@Override
public void testInit() {
@@ -111,10 +96,7 @@
CommandScheduler.getInstance().cancelAll();
}
- /**
- * This function is called periodically during test mode.
- */
+ /** This function is called periodically during test mode. */
@Override
- public void testPeriodic() {
- }
+ public void testPeriodic() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java
index 570a0ad..c798600 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java
@@ -1,14 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.schedulereventlogging;
+import static edu.wpi.first.wpilibj.XboxController.Button;
+
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.examples.schedulereventlogging.Constants.OIConstants;
import edu.wpi.first.wpilibj.shuffleboard.EventImportance;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj2.command.Command;
@@ -18,15 +18,11 @@
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import edu.wpi.first.wpilibj.examples.schedulereventlogging.Constants.OIConstants;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
-
/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * 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.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The driver's controller
@@ -38,9 +34,7 @@
private final CommandBase m_instantCommand2 = new InstantCommand();
private final CommandBase m_waitCommand = new WaitCommand(5);
- /**
- * The container for the robot. Contains subsystems, OI devices, and commands.
- */
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Set names of commands
m_instantCommand1.setName("Instant Command 1");
@@ -48,22 +42,31 @@
m_waitCommand.setName("Wait 5 Seconds Command");
// Set the scheduler to log Shuffleboard events for command initialize, interrupt, finish
- CommandScheduler.getInstance().onCommandInitialize(command -> Shuffleboard.addEventMarker(
- "Command initialized", command.getName(), EventImportance.kNormal));
- CommandScheduler.getInstance().onCommandInterrupt(command -> Shuffleboard.addEventMarker(
- "Command interrupted", command.getName(), EventImportance.kNormal));
- CommandScheduler.getInstance().onCommandFinish(command -> Shuffleboard.addEventMarker(
- "Command finished", command.getName(), EventImportance.kNormal));
+ CommandScheduler.getInstance()
+ .onCommandInitialize(
+ command ->
+ Shuffleboard.addEventMarker(
+ "Command initialized", command.getName(), EventImportance.kNormal));
+ CommandScheduler.getInstance()
+ .onCommandInterrupt(
+ command ->
+ Shuffleboard.addEventMarker(
+ "Command interrupted", command.getName(), EventImportance.kNormal));
+ CommandScheduler.getInstance()
+ .onCommandFinish(
+ command ->
+ Shuffleboard.addEventMarker(
+ "Command finished", command.getName(), EventImportance.kNormal));
// Configure the button bindings
configureButtonBindings();
}
/**
- * Use this method to define your button->command mappings. Buttons can be created by
+ * Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
- * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
- * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+ * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Run instant command 1 when the 'A' button is pressed
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Constants.java
index 2b73deb..5a76db6 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Constants.java
@@ -1,28 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.selectcommand;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants. This class should not be used for any other purpose. All constants should be
- * declared globally (i.e. public static). Do not put anything functional in this class.
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
/**
- * Example of an inner class. One can "import static [...].Constants.OIConstants.*" to gain
- * access to the constants contained within without having to preface the names with the class,
- * greatly reducing the amount of text required.
+ * Example of an inner class. One can "import static [...].Constants.OIConstants.*" to gain access
+ * to the constants contained within without having to preface the names with the class, greatly
+ * reducing the amount of text required.
*/
public static final class OIConstants {
// Example: the port of the driver's controller
- public static final int kDriverControllerPort = 1;
+ public static final int kDriverControllerPort = 0;
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Main.java
index a1104ac..1ad2aff 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.selectcommand;
@@ -15,8 +12,7 @@
* call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Robot.java
index 8d00173..8bd49e8 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.selectcommand;
@@ -37,8 +34,8 @@
* 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.
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
@@ -49,20 +46,14 @@
CommandScheduler.getInstance().run();
}
- /**
- * This function is called once each time the robot enters Disabled mode.
- */
+ /** This function is called once each time the robot enters Disabled mode. */
@Override
- public void disabledInit() {
- }
+ public void disabledInit() {}
@Override
- public void disabledPeriodic() {
- }
+ public void disabledPeriodic() {}
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
}
}
- /**
- * This function is called periodically during autonomous.
- */
+ /** This function is called periodically during autonomous. */
@Override
- public void autonomousPeriodic() {
- }
+ public void autonomousPeriodic() {}
@Override
public void teleopInit() {
@@ -98,12 +86,9 @@
}
}
- /**
- * This function is called periodically during operator control.
- */
+ /** This function is called periodically during operator control. */
@Override
- public void teleopPeriodic() {
- }
+ public void teleopPeriodic() {}
@Override
public void testInit() {
@@ -111,10 +96,7 @@
CommandScheduler.getInstance().cancelAll();
}
- /**
- * This function is called periodically during test mode.
- */
+ /** This function is called periodically during test mode. */
@Override
- public void testPeriodic() {
- }
+ public void testPeriodic() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java
index eb26912..b6aed81 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java
@@ -1,30 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.selectcommand;
-import java.util.Map;
-
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.SelectCommand;
+import java.util.Map;
/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * 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.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The enum used as keys for selecting the command to run.
private enum CommandSelector {
- ONE, TWO, THREE
+ ONE,
+ TWO,
+ THREE
}
// An example selector method for the selectcommand. Returns the selector that will select
@@ -43,10 +41,8 @@
Map.ofEntries(
Map.entry(CommandSelector.ONE, new PrintCommand("Command one was selected!")),
Map.entry(CommandSelector.TWO, new PrintCommand("Command two was selected!")),
- Map.entry(CommandSelector.THREE, new PrintCommand("Command three was selected!"))
- ),
- this::select
- );
+ Map.entry(CommandSelector.THREE, new PrintCommand("Command three was selected!"))),
+ this::select);
public RobotContainer() {
// Configure the button bindings
@@ -54,13 +50,12 @@
}
/**
- * Use this method to define your button->command mappings. Buttons can be created by
+ * Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
- private void configureButtonBindings() {
- }
+ private void configureButtonBindings() {}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Main.java
index d2283df..8a7a1c8 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.shuffleboard;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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 adc713d..eb6a6d9 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java
@@ -1,29 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.shuffleboard;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
public class Robot extends TimedRobot {
- private final DifferentialDrive m_tankDrive = new DifferentialDrive(new PWMVictorSPX(0),
- new PWMVictorSPX(1));
+ private final DifferentialDrive m_tankDrive =
+ new DifferentialDrive(new PWMSparkMax(0), new PWMSparkMax(1));
private final Encoder m_leftEncoder = new Encoder(0, 1);
private final Encoder m_rightEncoder = new Encoder(2, 3);
- private final PWMVictorSPX m_elevatorMotor = new PWMVictorSPX(2);
+ private final PWMSparkMax m_elevatorMotor = new PWMSparkMax(2);
private final AnalogPotentiometer m_elevatorPot = new AnalogPotentiometer(0);
private NetworkTableEntry m_maxSpeed;
@@ -31,20 +28,20 @@
public void robotInit() {
// Add a 'max speed' widget to a tab named 'Configuration', using a number slider
// The widget will be placed in the second column and row and will be TWO columns wide
- m_maxSpeed = Shuffleboard.getTab("Configuration")
- .add("Max Speed", 1)
- .withWidget("Number Slider")
- .withPosition(1, 1)
- .withSize(2, 1)
- .getEntry();
+ m_maxSpeed =
+ Shuffleboard.getTab("Configuration")
+ .add("Max Speed", 1)
+ .withWidget("Number Slider")
+ .withPosition(1, 1)
+ .withSize(2, 1)
+ .getEntry();
// Add the tank drive and encoders to a 'Drivebase' tab
ShuffleboardTab driveBaseTab = Shuffleboard.getTab("Drivebase");
driveBaseTab.add("Tank Drive", m_tankDrive);
// Put both encoders in a list layout
- ShuffleboardLayout encoders = driveBaseTab.getLayout("List Layout", "Encoders")
- .withPosition(0, 0)
- .withSize(2, 2);
+ ShuffleboardLayout encoders =
+ driveBaseTab.getLayout("List Layout", "Encoders").withPosition(0, 0).withSize(2, 2);
encoders.add("Left Encoder", m_leftEncoder);
encoders.add("Right Encoder", m_rightEncoder);
@@ -59,5 +56,4 @@
// Read the value of the 'max speed' widget from the dashboard
m_tankDrive.setMaxOutput(m_maxSpeed.getDouble(1.0));
}
-
}
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
new file mode 100644
index 0000000..ea80fd2
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
@@ -0,0 +1,163 @@
+// 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.simpledifferentialdrivesimulation;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+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;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class Drivetrain {
+ // 3 meters per second.
+ public static final double kMaxSpeed = 3.0;
+ // 1/2 rotation per second.
+ public static final double kMaxAngularSpeed = Math.PI;
+
+ private static final double kTrackWidth = 0.381 * 2;
+ private static final double kWheelRadius = 0.0508;
+ private static final int kEncoderResolution = -4096;
+
+ 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 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);
+
+ private final PIDController m_leftPIDController = new PIDController(8.5, 0, 0);
+ private final PIDController m_rightPIDController = new PIDController(8.5, 0, 0);
+
+ private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+ private final DifferentialDriveKinematics m_kinematics =
+ new DifferentialDriveKinematics(kTrackWidth);
+ private final DifferentialDriveOdometry m_odometry =
+ new DifferentialDriveOdometry(m_gyro.getRotation2d());
+
+ // Gains are for example purposes only - must be determined for your own
+ // robot!
+ private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
+
+ // Simulation classes help us simulate our robot
+ private final AnalogGyroSim m_gyroSim = new AnalogGyroSim(m_gyro);
+ private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder);
+ private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder);
+ private final Field2d m_fieldSim = new Field2d();
+ private final LinearSystem<N2, N2, N2> m_drivetrainSystem =
+ LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
+ private final DifferentialDrivetrainSim m_drivetrainSimulator =
+ new DifferentialDrivetrainSim(
+ m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth, kWheelRadius, null);
+
+ /** Subsystem constructor. */
+ public Drivetrain() {
+ // 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);
+
+ // Set the distance per pulse for the drive encoders. We can simply use the
+ // distance traveled for one rotation of the wheel divided by the encoder
+ // resolution.
+ m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+ m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+
+ m_rightGroup.setInverted(true);
+ SmartDashboard.putData("Field", m_fieldSim);
+ }
+
+ /** Sets speeds to the drivetrain motors. */
+ public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
+ var leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
+ var rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
+ double leftOutput =
+ m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
+ double rightOutput =
+ m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
+
+ m_leftGroup.setVoltage(leftOutput + leftFeedforward);
+ m_rightGroup.setVoltage(rightOutput + rightFeedforward);
+ }
+
+ /**
+ * Controls the robot using arcade drive.
+ *
+ * @param xSpeed the speed for the x axis
+ * @param rot the rotation
+ */
+ @SuppressWarnings("ParameterName")
+ public void drive(double xSpeed, double rot) {
+ setSpeeds(m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot)));
+ }
+
+ /** Update robot odometry. */
+ public void updateOdometry() {
+ m_odometry.update(
+ m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
+ }
+
+ /** Resets robot odometry. */
+ public void resetOdometry(Pose2d pose) {
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+ m_drivetrainSimulator.setPose(pose);
+ m_odometry.resetPosition(pose, m_gyro.getRotation2d());
+ }
+
+ /** Check the current robot pose. */
+ public Pose2d getPose() {
+ return m_odometry.getPoseMeters();
+ }
+
+ /** Update our simulation. This should be run every robot loop in simulation. */
+ public void simulationPeriodic() {
+ // To update our simulation, we set motor voltage inputs, update the
+ // simulation, and write the simulated positions and velocities to our
+ // simulated encoder and gyro. We negate the right side so that positive
+ // voltages make the right side move forward.
+ m_drivetrainSimulator.setInputs(
+ m_leftLeader.get() * RobotController.getInputVoltage(),
+ -m_rightLeader.get() * RobotController.getInputVoltage());
+ m_drivetrainSimulator.update(0.02);
+
+ m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
+ m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond());
+ m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters());
+ m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond());
+ m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
+ }
+
+ /** Update odometry - this should be run every robot loop. */
+ public void periodic() {
+ updateOdometry();
+ m_fieldSim.setRobotPose(m_odometry.getPoseMeters());
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Main.java
new file mode 100644
index 0000000..576b2d3
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/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.simpledifferentialdrivesimulation;
+
+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/simpledifferentialdrivesimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java
new file mode 100644
index 0000000..99178d4
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java
@@ -0,0 +1,86 @@
+// 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.simpledifferentialdrivesimulation;
+
+import edu.wpi.first.math.controller.RamseteController;
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.XboxController;
+import java.util.List;
+
+public class Robot extends TimedRobot {
+ private final XboxController m_controller = new XboxController(0);
+
+ // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+ // to 1.
+ private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
+ private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
+ private final Drivetrain m_drive = new Drivetrain();
+ private final RamseteController m_ramsete = new RamseteController();
+ private final Timer m_timer = new Timer();
+ private Trajectory m_trajectory;
+
+ @Override
+ public void robotInit() {
+ // Flush NetworkTables every loop. This ensures that robot pose and other values
+ // are sent during every iteration.
+ setNetworkTablesFlushEnabled(true);
+
+ m_trajectory =
+ TrajectoryGenerator.generateTrajectory(
+ new Pose2d(2, 2, new Rotation2d()),
+ List.of(),
+ new Pose2d(6, 4, new Rotation2d()),
+ new TrajectoryConfig(2, 2));
+ }
+
+ @Override
+ public void robotPeriodic() {
+ m_drive.periodic();
+ }
+
+ @Override
+ public void autonomousInit() {
+ m_timer.reset();
+ m_timer.start();
+ m_drive.resetOdometry(m_trajectory.getInitialPose());
+ }
+
+ @Override
+ public void autonomousPeriodic() {
+ double elapsed = m_timer.get();
+ Trajectory.State reference = m_trajectory.sample(elapsed);
+ ChassisSpeeds speeds = m_ramsete.calculate(m_drive.getPose(), reference);
+ m_drive.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond);
+ }
+
+ @Override
+ @SuppressWarnings("LocalVariableName")
+ public void teleopPeriodic() {
+ // Get the x speed. We are inverting this because Xbox controllers return
+ // negative values when we push forward.
+ double xSpeed = -m_speedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
+
+ // Get the rate of angular rotation. We are inverting this because we want a
+ // positive value when we pull to the left (remember, CCW is positive in
+ // mathematics). Xbox controllers return positive values when you pull to
+ // the right by default.
+ double rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
+ m_drive.drive(xSpeed, rot);
+ }
+
+ @Override
+ public void simulationPeriodic() {
+ m_drive.simulationPeriodic();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Main.java
index c62cb75..78218ad 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.solenoid;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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 8fc49bb..98fb3b6 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
@@ -1,14 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.solenoid;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.TimedRobot;
@@ -23,15 +21,15 @@
* state. Additionally, double solenoids take up two channels on your PCM whereas single solenoids
* only take a single channel.
*/
-
public class Robot extends TimedRobot {
private final Joystick m_stick = new Joystick(0);
// Solenoid corresponds to a single solenoid.
- private final Solenoid m_solenoid = new Solenoid(0);
+ private final Solenoid m_solenoid = new Solenoid(PneumaticsModuleType.CTREPCM, 0);
// DoubleSolenoid corresponds to a double solenoid.
- private final DoubleSolenoid m_doubleSolenoid = new DoubleSolenoid(1, 2);
+ private final DoubleSolenoid m_doubleSolenoid =
+ new DoubleSolenoid(PneumaticsModuleType.CTREPCM, 1, 2);
private static final int kSolenoidButton = 1;
private static final int kDoubleSolenoidForward = 2;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Main.java
index c8a6fb1..204e8e3 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.statespacearm;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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 508102d..9877906 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
@@ -1,33 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.statespacearm;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.LinearQuadraticRegulator;
+import edu.wpi.first.math.estimator.KalmanFilter;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.LinearSystemLoop;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
-import edu.wpi.first.wpilibj.estimator.KalmanFilter;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.LinearSystemLoop;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
- * This is a sample program to demonstrate how to use a state-space controller
- * to control an arm.
+ * This is a sample program to demonstrate how to use a state-space controller to control an arm.
*/
public class Robot extends TimedRobot {
private static final int kMotorPort = 0;
@@ -45,8 +41,10 @@
// the motors, this number should be greater than one.
private static final double kArmGearing = 10.0;
- private final TrapezoidProfile.Constraints m_constraints = new TrapezoidProfile.Constraints(
- Units.degreesToRadians(45), Units.degreesToRadians(90)); // Max arm speed and acceleration.
+ private final TrapezoidProfile.Constraints m_constraints =
+ new TrapezoidProfile.Constraints(
+ Units.degreesToRadians(45),
+ Units.degreesToRadians(90)); // Max arm speed and acceleration.
private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State();
// The plant holds a state-space model of our arm. This system has the following properties:
@@ -55,47 +53,45 @@
// Inputs (what we can "put in"): [voltage], in volts.
// Outputs (what we can measure): [position], in radians.
private final LinearSystem<N2, N1, N1> m_armPlant =
- LinearSystemId.createSingleJointedArmSystem(
- DCMotor.getNEO(2),
- kArmMOI,
- kArmGearing);
+ LinearSystemId.createSingleJointedArmSystem(DCMotor.getNEO(2), kArmMOI, kArmGearing);
// The observer fuses our encoder data and voltage inputs to reject noise.
- private final KalmanFilter<N2, N1, N1> m_observer = new KalmanFilter<>(
- Nat.N2(), Nat.N1(),
- m_armPlant,
- VecBuilder.fill(0.015, 0.17), // How accurate we
- // think our model is, in radians and radians/sec
- VecBuilder.fill(0.01), // How accurate we think our encoder position
- // data is. In this case we very highly trust our encoder position reading.
- 0.020);
+ private final KalmanFilter<N2, N1, N1> m_observer =
+ new KalmanFilter<>(
+ Nat.N2(),
+ Nat.N1(),
+ m_armPlant,
+ VecBuilder.fill(0.015, 0.17), // How accurate we
+ // think our model is, in radians and radians/sec
+ VecBuilder.fill(0.01), // How accurate we think our encoder position
+ // data is. In this case we very highly trust our encoder position reading.
+ 0.020);
// A LQR uses feedback to create voltage commands.
- private final LinearQuadraticRegulator<N2, N1, N1> m_controller
- = new LinearQuadraticRegulator<>(m_armPlant,
- VecBuilder.fill(Units.degreesToRadians(1.0), Units.degreesToRadians(10.0)), // qelms.
- // Position and velocity error tolerances, in radians and radians per second. Decrease this
- // to more heavily penalize state excursion, or make the controller behave more
- // aggressively. In this example we weight position much more highly than velocity, but this
- // can be tuned to balance the two.
- VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
- // heavily penalize control effort, or make the controller less aggressive. 12 is a good
- // starting point because that is the (approximate) maximum voltage of a battery.
- 0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
+ private final LinearQuadraticRegulator<N2, N1, N1> m_controller =
+ new LinearQuadraticRegulator<>(
+ m_armPlant,
+ VecBuilder.fill(Units.degreesToRadians(1.0), Units.degreesToRadians(10.0)), // qelms.
+ // Position and velocity error tolerances, in radians and radians per second. Decrease
+ // this
+ // to more heavily penalize state excursion, or make the controller behave more
+ // aggressively. In this example we weight position much more highly than velocity, but
+ // this
+ // can be tuned to balance the two.
+ VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
+ // heavily penalize control effort, or make the controller less aggressive. 12 is a good
+ // starting point because that is the (approximate) maximum voltage of a battery.
+ 0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
// lower if using notifiers.
// The state-space loop combines a controller, observer, feedforward and plant for easy control.
- private final LinearSystemLoop<N2, N1, N1> m_loop = new LinearSystemLoop<>(
- m_armPlant,
- m_controller,
- m_observer,
- 12.0,
- 0.020);
+ private final LinearSystemLoop<N2, N1, N1> m_loop =
+ new LinearSystemLoop<>(m_armPlant, m_controller, m_observer, 12.0, 0.020);
// An encoder set up to measure arm position in radians.
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
- private final SpeedController m_motor = new PWMVictorSPX(kMotorPort);
+ private final MotorController m_motor = new PWMSparkMax(kMotorPort);
// A joystick to read the trigger from.
private final Joystick m_joystick = new Joystick(kJoystickPort);
@@ -112,8 +108,8 @@
m_loop.reset(VecBuilder.fill(m_encoder.getDistance(), m_encoder.getRate()));
// Reset our last reference to the current state.
- m_lastProfiledReference = new TrapezoidProfile.State(m_encoder.getDistance(),
- m_encoder.getRate());
+ m_lastProfiledReference =
+ new TrapezoidProfile.State(m_encoder.getDistance(), m_encoder.getRate());
}
@Override
@@ -129,8 +125,8 @@
goal = new TrapezoidProfile.State(kLoweredPosition, 0.0);
}
// Step our TrapezoidalProfile forward 20ms and set it as our next reference
- m_lastProfiledReference = (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference))
- .calculate(0.020);
+ m_lastProfiledReference =
+ (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference)).calculate(0.020);
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/statespacedifferentialdrivesimulation/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java
index b86bd3f..2177bd7 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java
@@ -1,22 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.statespacedifferentialdrivesimulation;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
/**
* 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.
+ * 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.
@@ -28,20 +25,20 @@
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
- public static final int[] kLeftEncoderPorts = new int[]{0, 1};
- public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+ public static final int[] kRightEncoderPorts = new int[] {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
public static final double kTrackwidthMeters = 0.69;
public static final DifferentialDriveKinematics kDriveKinematics =
- new DifferentialDriveKinematics(kTrackwidthMeters);
+ new DifferentialDriveKinematics(kTrackwidthMeters);
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterMeters = 0.15;
public static final double kEncoderDistancePerPulse =
- // Assumes the encoders are directly mounted on the wheel shafts
- (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
public static final boolean kGyroReversed = true;
@@ -62,15 +59,18 @@
public static final double kaVoltSecondsSquaredPerRadian = 0.3;
public static final LinearSystem<N2, N2, N2> kDrivetrainPlant =
- LinearSystemId.identifyDrivetrainSystem(kvVoltSecondsPerMeter, kaVoltSecondsSquaredPerMeter,
- kvVoltSecondsPerRadian, kaVoltSecondsSquaredPerRadian);
+ LinearSystemId.identifyDrivetrainSystem(
+ kvVoltSecondsPerMeter,
+ kaVoltSecondsSquaredPerMeter,
+ kvVoltSecondsPerRadian,
+ kaVoltSecondsSquaredPerRadian);
// Example values only -- use what's on your physical robot!
public static final DCMotor kDriveGearbox = DCMotor.getCIM(2);
public static final double kDriveGearing = 8;
// Example value only - as above, this must be tuned for your drive!
- public static final double kPDriveVel = 0.1;
+ public static final double kPDriveVel = 8.5;
}
public static final class OIConstants {
@@ -79,7 +79,7 @@
public static final class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 3;
- public static final double kMaxAccelerationMetersPerSecondSquared = 6;
+ public static final double kMaxAccelerationMetersPerSecondSquared = 3;
// Reasonable baseline values for a RAMSETE follower in units of meters and seconds
public static final double kRamseteB = 2;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java
index 5b041fb..bccc25a 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.statespacedifferentialdrivesimulation;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java
index 485dfc3..75700c4 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java
@@ -1,21 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.statespacedifferentialdrivesimulation;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.simulation.RoboRioSim;
import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
- * This is a sample program to demonstrate the use of state-space classes in robot simulation.
- * This robot has a flywheel, elevator, arm and differential drivetrain, and interfaces with
- * the sim GUI's {@link edu.wpi.first.wpilibj.simulation.Field2d} class.
+ * This is a sample program to demonstrate the use of state-space classes in robot simulation. This
+ * robot has a flywheel, elevator, arm and differential drivetrain, and interfaces with the sim
+ * GUI's {@link edu.wpi.first.wpilibj.simulation.Field2d} class.
*/
public class Robot extends TimedRobot {
private RobotContainer m_robotContainer;
@@ -29,6 +26,10 @@
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
+
+ // Flush NetworkTables every loop. This ensures that robot pose and other values
+ // are sent during every loop iteration.
+ setNetworkTablesFlushEnabled(true);
}
@Override
@@ -55,5 +56,6 @@
@Override
public void disabledInit() {
CommandScheduler.getInstance().cancelAll();
+ m_robotContainer.zeroAllOutputs();
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java
index 0790a40..5cbe38b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java
@@ -1,49 +1,43 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.statespacedifferentialdrivesimulation;
-import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.RamseteController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+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.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.RamseteController;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.XboxController.Button;
import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.subsystems.DriveSubsystem;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
import java.util.List;
-import static edu.wpi.first.wpilibj.XboxController.Button;
-
/**
- * 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 Robot
- * periodic methods (other than the scheduler calls). Instead, the structure of the robot
- * (including subsystems, commands, and button mappings) should be declared here.
+ * 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 Robot periodic
+ * methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
// The driver's controller
- XboxController m_driverController = new XboxController(Constants.OIConstants.kDriverControllerPort);
+ XboxController m_driverController =
+ new XboxController(Constants.OIConstants.kDriverControllerPort);
- /**
- * The container for the robot. Contains subsystems, OI devices, and commands.
- */
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
@@ -53,41 +47,46 @@
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
- new RunCommand(() -> m_robotDrive
- .arcadeDrive(-m_driverController.getY(GenericHID.Hand.kRight),
- m_driverController.getX(GenericHID.Hand.kLeft)), m_robotDrive));
-
+ new RunCommand(
+ () ->
+ m_robotDrive.arcadeDrive(
+ -m_driverController.getRightY(), m_driverController.getLeftX()),
+ m_robotDrive));
}
/**
- * Use this method to define your button->command mappings. Buttons can be created by
- * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
* {@link JoystickButton}.
*/
private void configureButtonBindings() {
// Drive at half speed when the right bumper is held
- new JoystickButton(m_driverController, Button.kBumperRight.value)
+ new JoystickButton(m_driverController, Button.kRightBumper.value)
.whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
-
}
public DriveSubsystem getRobotDrive() {
return m_robotDrive;
}
+ /** Zeros the outputs of all subsystems. */
+ public void zeroAllOutputs() {
+ m_robotDrive.tankDriveVolts(0, 0);
+ }
+
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
-
// Create a voltage constraint to ensure we don't accelerate too fast
var autoVoltageConstraint =
new DifferentialDriveVoltageConstraint(
- new SimpleMotorFeedforward(Constants.DriveConstants.ksVolts,
+ new SimpleMotorFeedforward(
+ Constants.DriveConstants.ksVolts,
Constants.DriveConstants.kvVoltSecondsPerMeter,
Constants.DriveConstants.kaVoltSecondsSquaredPerMeter),
Constants.DriveConstants.kDriveKinematics,
@@ -95,40 +94,43 @@
// Create config for trajectory
TrajectoryConfig config =
- new TrajectoryConfig(Constants.AutoConstants.kMaxSpeedMetersPerSecond,
- Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared)
+ new TrajectoryConfig(
+ Constants.AutoConstants.kMaxSpeedMetersPerSecond,
+ Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(Constants.DriveConstants.kDriveKinematics)
// Apply the voltage constraint
.addConstraint(autoVoltageConstraint);
// An example trajectory to follow. All units in meters.
- Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
- // Start at the origin facing the +X direction
- new Pose2d(0, 0, new Rotation2d(0)),
- // Pass through these two interior waypoints, making an 's' curve path
- List.of(),
- // End 3 meters straight ahead of where we started, facing forward
- new Pose2d(6, 6, new Rotation2d(0)),
- // Pass config
- config
- );
+ Trajectory exampleTrajectory =
+ TrajectoryGenerator.generateTrajectory(
+ // Start at (1, 2) facing the +X direction
+ new Pose2d(1, 2, new Rotation2d(0)),
+ // Pass through these two interior waypoints, making an 's' curve path
+ List.of(new Translation2d(2, 3), new Translation2d(3, 1)),
+ // End 3 meters straight ahead of where we started, facing forward
+ new Pose2d(4, 2, new Rotation2d(0)),
+ // Pass config
+ config);
- RamseteCommand ramseteCommand = new RamseteCommand(
- exampleTrajectory,
- m_robotDrive::getPose,
- new RamseteController(Constants.AutoConstants.kRamseteB, Constants.AutoConstants.kRamseteZeta),
- new SimpleMotorFeedforward(Constants.DriveConstants.ksVolts,
- Constants.DriveConstants.kvVoltSecondsPerMeter,
- Constants.DriveConstants.kaVoltSecondsSquaredPerMeter),
- Constants.DriveConstants.kDriveKinematics,
- m_robotDrive::getWheelSpeeds,
- new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0),
- new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0),
- // RamseteCommand passes volts to the callback
- m_robotDrive::tankDriveVolts,
- m_robotDrive
- );
+ RamseteCommand ramseteCommand =
+ new RamseteCommand(
+ exampleTrajectory,
+ m_robotDrive::getPose,
+ new RamseteController(
+ Constants.AutoConstants.kRamseteB, Constants.AutoConstants.kRamseteZeta),
+ new SimpleMotorFeedforward(
+ Constants.DriveConstants.ksVolts,
+ Constants.DriveConstants.kvVoltSecondsPerMeter,
+ Constants.DriveConstants.kaVoltSecondsSquaredPerMeter),
+ Constants.DriveConstants.kDriveKinematics,
+ m_robotDrive::getWheelSpeeds,
+ new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0),
+ new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0),
+ // RamseteCommand passes volts to the callback
+ m_robotDrive::tankDriveVolts,
+ m_robotDrive);
// Reset odometry to starting pose of trajectory.
m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
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 aaa0d8c..98037c3 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
@@ -1,61 +1,61 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.statespacedifferentialdrivesimulation.subsystems;
-import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+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.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj.SPI;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.Constants;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
-import edu.wpi.first.wpilibj.simulation.EncoderSim;
-import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
+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;
-import edu.wpi.first.wpilibj.simulation.Field2d;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
- private final SpeedControllerGroup m_leftMotors =
- new SpeedControllerGroup(new PWMVictorSPX(Constants.DriveConstants.kLeftMotor1Port),
- new PWMVictorSPX(Constants.DriveConstants.kLeftMotor2Port));
+ private final MotorControllerGroup m_leftMotors =
+ new MotorControllerGroup(
+ new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+ new PWMSparkMax(DriveConstants.kLeftMotor2Port));
// The motors on the right side of the drive.
- private final SpeedControllerGroup m_rightMotors =
- new SpeedControllerGroup(new PWMVictorSPX(Constants.DriveConstants.kRightMotor1Port),
- new PWMVictorSPX(Constants.DriveConstants.kRightMotor2Port));
+ private final MotorControllerGroup m_rightMotors =
+ new MotorControllerGroup(
+ new PWMSparkMax(DriveConstants.kRightMotor1Port),
+ new PWMSparkMax(DriveConstants.kRightMotor2Port));
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
// The left-side drive encoder
private final Encoder m_leftEncoder =
- new Encoder(Constants.DriveConstants.kLeftEncoderPorts[0],
- Constants.DriveConstants.kLeftEncoderPorts[1],
- Constants.DriveConstants.kLeftEncoderReversed);
+ new Encoder(
+ DriveConstants.kLeftEncoderPorts[0],
+ DriveConstants.kLeftEncoderPorts[1],
+ DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
- new Encoder(Constants.DriveConstants.kRightEncoderPorts[0],
- Constants.DriveConstants.kRightEncoderPorts[1],
- Constants.DriveConstants.kRightEncoderReversed);
+ new Encoder(
+ DriveConstants.kRightEncoderPorts[0],
+ DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
// The gyro sensor
- private final Gyro m_gyro = new ADXRS450_Gyro();
+ private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
// Odometry class for tracking robot pose
private final DifferentialDriveOdometry m_odometry;
@@ -64,48 +64,54 @@
public DifferentialDrivetrainSim m_drivetrainSimulator;
private EncoderSim m_leftEncoderSim;
private EncoderSim m_rightEncoderSim;
- // The Field2d class simulates the field in the sim GUI. Note that we can have only one
- // instance!
+ // The Field2d class shows the field in the sim GUI
private Field2d m_fieldSim;
- private SimDouble m_gyroAngleSim;
+ private ADXRS450_GyroSim m_gyroSim;
- /**
- * Creates a new DriveSubsystem.
- */
+ /** Creates a new DriveSubsystem. */
public DriveSubsystem() {
+ // 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);
+
// Sets the distance per pulse for the encoders
- m_leftEncoder.setDistancePerPulse(Constants.DriveConstants.kEncoderDistancePerPulse);
- m_rightEncoder.setDistancePerPulse(Constants.DriveConstants.kEncoderDistancePerPulse);
+ m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
resetEncoders();
m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));
if (RobotBase.isSimulation()) { // If our robot is simulated
// This class simulates our drivetrain's motion around the field.
- m_drivetrainSimulator = new DifferentialDrivetrainSim(
- Constants.DriveConstants.kDrivetrainPlant,
- Constants.DriveConstants.kDriveGearbox,
- Constants.DriveConstants.kDriveGearing,
- Constants.DriveConstants.kTrackwidthMeters,
- Constants.DriveConstants.kWheelDiameterMeters / 2.0);
+ m_drivetrainSimulator =
+ new DifferentialDrivetrainSim(
+ DriveConstants.kDrivetrainPlant,
+ DriveConstants.kDriveGearbox,
+ DriveConstants.kDriveGearing,
+ DriveConstants.kTrackwidthMeters,
+ DriveConstants.kWheelDiameterMeters / 2.0,
+ VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005));
// The encoder and gyro angle sims let us set simulated sensor readings
m_leftEncoderSim = new EncoderSim(m_leftEncoder);
m_rightEncoderSim = new EncoderSim(m_rightEncoder);
- m_gyroAngleSim =
- new SimDeviceSim("ADXRS450_Gyro" + "[" + SPI.Port.kOnboardCS0.value + "]")
- .getDouble("Angle");
+ m_gyroSim = new ADXRS450_GyroSim(m_gyro);
// the Field2d class lets us visualize our robot in the simulation GUI.
m_fieldSim = new Field2d();
+ SmartDashboard.putData("Field", m_fieldSim);
}
}
@Override
public void periodic() {
// Update the odometry in the periodic block
- m_odometry.update(Rotation2d.fromDegrees(getHeading()), m_leftEncoder.getDistance(),
- m_rightEncoder.getDistance());
+ m_odometry.update(
+ Rotation2d.fromDegrees(getHeading()),
+ m_leftEncoder.getDistance(),
+ m_rightEncoder.getDistance());
+ m_fieldSim.setRobotPose(getPose());
}
@Override
@@ -114,23 +120,21 @@
// and write the simulated positions and velocities to our simulated encoder and gyro.
// 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_drivetrainSimulator.setInputs(
+ m_leftMotors.get() * RobotController.getBatteryVoltage(),
+ -m_rightMotors.get() * RobotController.getBatteryVoltage());
m_drivetrainSimulator.update(0.020);
- m_leftEncoderSim.setDistance(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kLeftPosition));
- m_leftEncoderSim.setRate(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kLeftVelocity));
- m_rightEncoderSim.setDistance(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kRightPosition));
- m_rightEncoderSim.setRate(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kRightVelocity));
- m_gyroAngleSim.set(-m_drivetrainSimulator.getHeading().getDegrees());
-
- m_fieldSim.setRobotPose(getPose());
+ m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
+ m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond());
+ m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters());
+ m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond());
+ m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
}
/**
- * Returns the current being drawn by the drivetrain. This works in SIMULATION ONLY!
- * If you want it to work elsewhere, use the code in
- * {@link DifferentialDrivetrainSim#getCurrentDrawAmps()}
+ * Returns the current being drawn by the drivetrain. This works in SIMULATION ONLY! If you want
+ * it to work elsewhere, use the code in {@link DifferentialDrivetrainSim#getCurrentDrawAmps()}
*
* @return The drawn current in Amps.
*/
@@ -180,13 +184,12 @@
/**
* Controls the left and right sides of the drive directly with voltages.
*
- * @param leftVolts the commanded left output
+ * @param leftVolts the commanded left output
* @param rightVolts the commanded right output
*/
public void tankDriveVolts(double leftVolts, double rightVolts) {
var batteryVoltage = RobotController.getBatteryVoltage();
- if (Math.max(Math.abs(leftVolts), Math.abs(rightVolts))
- > batteryVoltage) {
+ if (Math.max(Math.abs(leftVolts), Math.abs(rightVolts)) > batteryVoltage) {
leftVolts *= batteryVoltage / 12.0;
rightVolts *= batteryVoltage / 12.0;
}
@@ -195,9 +198,7 @@
m_drive.feed();
}
- /**
- * Resets the drive encoders to currently read a position of 0.
- */
+ /** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
@@ -231,7 +232,7 @@
}
/**
- * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
@@ -239,9 +240,7 @@
m_drive.setMaxOutput(maxOutput);
}
- /**
- * Zeroes the heading of the robot.
- */
+ /** Zeroes the heading of the robot. */
public void zeroHeading() {
m_gyro.reset();
}
@@ -252,7 +251,6 @@
* @return the robot's heading in degrees, from -180 to 180
*/
public double getHeading() {
- return Math.IEEEremainder(m_gyro.getAngle(), 360) * (Constants.DriveConstants.kGyroReversed ? -1.0 : 1.0);
+ return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
}
-
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Main.java
index 03dff05..a4bad42 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.statespaceelevator;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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 85cbff4..f1aca19 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
@@ -1,33 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.statespaceelevator;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.LinearQuadraticRegulator;
+import edu.wpi.first.math.estimator.KalmanFilter;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.LinearSystemLoop;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
-import edu.wpi.first.wpilibj.estimator.KalmanFilter;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.LinearSystemLoop;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
- * This is a sample program to demonstrate how to use a state-space controller
- * to control an elevator.
+ * This is a sample program to demonstrate how to use a state-space controller to control an
+ * elevator.
*/
public class Robot extends TimedRobot {
private static final int kMotorPort = 0;
@@ -46,8 +43,9 @@
// the motors, this number should be greater than one.
private static final double kElevatorGearing = 6.0;
- private final TrapezoidProfile.Constraints m_constraints = new TrapezoidProfile.Constraints(
- Units.feetToMeters(3.0), Units.feetToMeters(6.0)); // Max elevator speed and acceleration.
+ private final TrapezoidProfile.Constraints m_constraints =
+ new TrapezoidProfile.Constraints(
+ Units.feetToMeters(3.0), Units.feetToMeters(6.0)); // Max elevator speed and acceleration.
private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State();
/* The plant holds a state-space model of our elevator. This system has the following properties:
@@ -58,49 +56,45 @@
This elevator is driven by two NEO motors.
*/
- private final LinearSystem<N2, N1, N1> m_elevatorPlant = LinearSystemId.createElevatorSystem(
- DCMotor.getNEO(2),
- kCarriageMass,
- kDrumRadius,
- kElevatorGearing
- );
+ private final LinearSystem<N2, N1, N1> m_elevatorPlant =
+ LinearSystemId.createElevatorSystem(
+ DCMotor.getNEO(2), kCarriageMass, kDrumRadius, kElevatorGearing);
// The observer fuses our encoder data and voltage inputs to reject noise.
- private final KalmanFilter<N2, N1, N1> m_observer = new KalmanFilter<>(
- Nat.N2(), Nat.N1(),
- m_elevatorPlant,
- VecBuilder.fill(Units.inchesToMeters(2), Units.inchesToMeters(40)), // How accurate we
- // think our model is, in meters and meters/second.
- VecBuilder.fill(0.001), // How accurate we think our encoder position
- // data is. In this case we very highly trust our encoder position reading.
- 0.020);
+ private final KalmanFilter<N2, N1, N1> m_observer =
+ new KalmanFilter<>(
+ Nat.N2(),
+ Nat.N1(),
+ m_elevatorPlant,
+ VecBuilder.fill(Units.inchesToMeters(2), Units.inchesToMeters(40)), // How accurate we
+ // think our model is, in meters and meters/second.
+ VecBuilder.fill(0.001), // How accurate we think our encoder position
+ // data is. In this case we very highly trust our encoder position reading.
+ 0.020);
// A LQR uses feedback to create voltage commands.
- private final LinearQuadraticRegulator<N2, N1, N1> m_controller
- = new LinearQuadraticRegulator<>(m_elevatorPlant,
- VecBuilder.fill(Units.inchesToMeters(1.0), Units.inchesToMeters(10.0)), // qelms. Position
- // and velocity error tolerances, in meters and meters per second. Decrease this to more
- // heavily penalize state excursion, or make the controller behave more aggressively. In
- // this example we weight position much more highly than velocity, but this can be
- // tuned to balance the two.
- VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
- // heavily penalize control effort, or make the controller less aggressive. 12 is a good
- // starting point because that is the (approximate) maximum voltage of a battery.
- 0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
+ private final LinearQuadraticRegulator<N2, N1, N1> m_controller =
+ new LinearQuadraticRegulator<>(
+ m_elevatorPlant,
+ VecBuilder.fill(Units.inchesToMeters(1.0), Units.inchesToMeters(10.0)), // qelms. Position
+ // and velocity error tolerances, in meters and meters per second. Decrease this to more
+ // heavily penalize state excursion, or make the controller behave more aggressively. In
+ // this example we weight position much more highly than velocity, but this can be
+ // tuned to balance the two.
+ VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
+ // heavily penalize control effort, or make the controller less aggressive. 12 is a good
+ // starting point because that is the (approximate) maximum voltage of a battery.
+ 0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
// lower if using notifiers.
// The state-space loop combines a controller, observer, feedforward and plant for easy control.
- private final LinearSystemLoop<N2, N1, N1> m_loop = new LinearSystemLoop<>(
- m_elevatorPlant,
- m_controller,
- m_observer,
- 12.0,
- 0.020);
+ private final LinearSystemLoop<N2, N1, N1> m_loop =
+ new LinearSystemLoop<>(m_elevatorPlant, m_controller, m_observer, 12.0, 0.020);
// An encoder set up to measure elevator height in meters.
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
- private final SpeedController m_motor = new PWMVictorSPX(kMotorPort);
+ private final MotorController m_motor = new PWMSparkMax(kMotorPort);
// A joystick to read the trigger from.
private final Joystick m_joystick = new Joystick(kJoystickPort);
@@ -117,8 +111,8 @@
m_loop.reset(VecBuilder.fill(m_encoder.getDistance(), m_encoder.getRate()));
// Reset our last reference to the current state.
- m_lastProfiledReference = new TrapezoidProfile.State(m_encoder.getDistance(),
- m_encoder.getRate());
+ m_lastProfiledReference =
+ new TrapezoidProfile.State(m_encoder.getDistance(), m_encoder.getRate());
}
@Override
@@ -134,8 +128,8 @@
goal = new TrapezoidProfile.State(kLowGoalPosition, 0.0);
}
// Step our TrapezoidalProfile forward 20ms and set it as our next reference
- m_lastProfiledReference = (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference))
- .calculate(0.020);
+ m_lastProfiledReference =
+ (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference)).calculate(0.020);
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/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Main.java
index 3673388..9e48d52 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.statespaceflywheel;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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 f9d1131..06e8d29 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
@@ -1,31 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.statespaceflywheel;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.LinearQuadraticRegulator;
+import edu.wpi.first.math.estimator.KalmanFilter;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.LinearSystemLoop;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
-import edu.wpi.first.wpilibj.estimator.KalmanFilter;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.LinearSystemLoop;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
- * This is a sample program to demonstrate how to use a state-space controller
- * to control a flywheel.
+ * This is a sample program to demonstrate how to use a state-space controller to control a
+ * flywheel.
*/
public class Robot extends TimedRobot {
private static final int kMotorPort = 0;
@@ -45,44 +42,42 @@
// States: [velocity], in radians per second.
// Inputs (what we can "put in"): [voltage], in volts.
// Outputs (what we can measure): [velocity], in radians per second.
- private final LinearSystem<N1, N1, N1> m_flywheelPlant = LinearSystemId.createFlywheelSystem(
- DCMotor.getNEO(2),
- kFlywheelMomentOfInertia,
- kFlywheelGearing);
+ private final LinearSystem<N1, N1, N1> m_flywheelPlant =
+ LinearSystemId.createFlywheelSystem(
+ DCMotor.getNEO(2), kFlywheelMomentOfInertia, kFlywheelGearing);
// The observer fuses our encoder data and voltage inputs to reject noise.
- private final KalmanFilter<N1, N1, N1> m_observer = new KalmanFilter<>(
- Nat.N1(), Nat.N1(),
- m_flywheelPlant,
- VecBuilder.fill(3.0), // How accurate we think our model is
- VecBuilder.fill(0.01), // How accurate we think our encoder
- // data is
- 0.020);
+ private final KalmanFilter<N1, N1, N1> m_observer =
+ new KalmanFilter<>(
+ Nat.N1(),
+ Nat.N1(),
+ m_flywheelPlant,
+ VecBuilder.fill(3.0), // How accurate we think our model is
+ VecBuilder.fill(0.01), // How accurate we think our encoder
+ // data is
+ 0.020);
// A LQR uses feedback to create voltage commands.
- private final LinearQuadraticRegulator<N1, N1, N1> m_controller
- = new LinearQuadraticRegulator<>(m_flywheelPlant,
- VecBuilder.fill(8.0), // qelms. Velocity error tolerance, in radians per second. Decrease
- // this to more heavily penalize state excursion, or make the controller behave more
- // aggressively.
- VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
- // heavily penalize control effort, or make the controller less aggressive. 12 is a good
- // starting point because that is the (approximate) maximum voltage of a battery.
- 0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
+ private final LinearQuadraticRegulator<N1, N1, N1> m_controller =
+ new LinearQuadraticRegulator<>(
+ m_flywheelPlant,
+ VecBuilder.fill(8.0), // qelms. Velocity error tolerance, in radians per second. Decrease
+ // this to more heavily penalize state excursion, or make the controller behave more
+ // aggressively.
+ VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
+ // heavily penalize control effort, or make the controller less aggressive. 12 is a good
+ // starting point because that is the (approximate) maximum voltage of a battery.
+ 0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
// lower if using notifiers.
// The state-space loop combines a controller, observer, feedforward and plant for easy control.
- private final LinearSystemLoop<N1, N1, N1> m_loop = new LinearSystemLoop<>(
- m_flywheelPlant,
- m_controller,
- m_observer,
- 12.0,
- 0.020);
+ private final LinearSystemLoop<N1, N1, N1> m_loop =
+ new LinearSystemLoop<>(m_flywheelPlant, m_controller, m_observer, 12.0, 0.020);
// An encoder set up to measure flywheel velocity in radians per second.
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
- private final SpeedController m_motor = new PWMVictorSPX(kMotorPort);
+ private final MotorController m_motor = new PWMSparkMax(kMotorPort);
// A joystick to read the trigger from.
private final Joystick m_joystick = new Joystick(kJoystickPort);
@@ -90,8 +85,7 @@
@Override
public void robotInit() {
// We go 2 pi radians per 4096 clicks.
- m_encoder.setDistancePerPulse(
- 2.0 * Math.PI / 4096.0);
+ m_encoder.setDistancePerPulse(2.0 * Math.PI / 4096.0);
}
@Override
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Main.java
index e487e9f..67941f6 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.statespaceflywheelsysid;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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 7cd773b..26ee8f7 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
@@ -1,30 +1,27 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.statespaceflywheelsysid;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.LinearQuadraticRegulator;
+import edu.wpi.first.math.estimator.KalmanFilter;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.LinearSystemLoop;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
-import edu.wpi.first.wpilibj.estimator.KalmanFilter;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.LinearSystemLoop;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
- * This is a sample program to demonstrate how to use a state-space controller
- * to control a flywheel.
+ * This is a sample program to demonstrate how to use a state-space controller to control a
+ * flywheel.
*/
public class Robot extends TimedRobot {
private static final int kMotorPort = 0;
@@ -46,37 +43,36 @@
// Outputs (what we can measure): [velocity], in radians per second.
//
// The Kv and Ka constants are found using the FRC Characterization toolsuite.
- private final LinearSystem<N1, N1, N1> m_flywheelPlant = LinearSystemId.identifyVelocitySystem(
- kFlywheelKv, kFlywheelKa);
+ private final LinearSystem<N1, N1, N1> m_flywheelPlant =
+ LinearSystemId.identifyVelocitySystem(kFlywheelKv, kFlywheelKa);
// The observer fuses our encoder data and voltage inputs to reject noise.
- private final KalmanFilter<N1, N1, N1> m_observer = new KalmanFilter<>(
- Nat.N1(), Nat.N1(),
- m_flywheelPlant,
- VecBuilder.fill(3.0), // How accurate we think our model is
- VecBuilder.fill(0.01), // How accurate we think our encoder
- // data is
- 0.020);
+ private final KalmanFilter<N1, N1, N1> m_observer =
+ new KalmanFilter<>(
+ Nat.N1(),
+ Nat.N1(),
+ m_flywheelPlant,
+ VecBuilder.fill(3.0), // How accurate we think our model is
+ VecBuilder.fill(0.01), // How accurate we think our encoder
+ // data is
+ 0.020);
// A LQR uses feedback to create voltage commands.
- private final LinearQuadraticRegulator<N1, N1, N1> m_controller
- = new LinearQuadraticRegulator<>(m_flywheelPlant,
- VecBuilder.fill(8.0), // Velocity error tolerance
- VecBuilder.fill(12.0), // Control effort (voltage) tolerance
- 0.020);
+ private final LinearQuadraticRegulator<N1, N1, N1> m_controller =
+ new LinearQuadraticRegulator<>(
+ m_flywheelPlant,
+ VecBuilder.fill(8.0), // Velocity error tolerance
+ VecBuilder.fill(12.0), // Control effort (voltage) tolerance
+ 0.020);
// The state-space loop combines a controller, observer, feedforward and plant for easy control.
- private final LinearSystemLoop<N1, N1, N1> m_loop = new LinearSystemLoop<>(
- m_flywheelPlant,
- m_controller,
- m_observer,
- 12.0,
- 0.020);
+ private final LinearSystemLoop<N1, N1, N1> m_loop =
+ new LinearSystemLoop<>(m_flywheelPlant, m_controller, m_observer, 12.0, 0.020);
// An encoder set up to measure flywheel velocity in radians per second.
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
- private final SpeedController m_motor = new PWMVictorSPX(kMotorPort);
+ private final MotorController m_motor = new PWMSparkMax(kMotorPort);
// A joystick to read the trigger from.
private final Joystick m_joystick = new Joystick(kJoystickPort);
@@ -84,8 +80,7 @@
@Override
public void robotInit() {
// We go 2 pi radians per 4096 clicks.
- m_encoder.setDistancePerPulse(
- 2.0 * Math.PI / 4096.0);
+ m_encoder.setDistancePerPulse(2.0 * Math.PI / 4096.0);
}
@Override
@@ -96,7 +91,6 @@
@Override
public void teleopPeriodic() {
-
// Sets the target speed of our flywheel. This is similar to setting the setpoint of a
// PID controller.
if (m_joystick.getTriggerPressed()) {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
index 5f1227a..3575266 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
@@ -1,21 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.swervebot;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.wpilibj.AnalogGyro;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry;
-/**
- * Represents a swerve drive style drivetrain.
- */
+/** Represents a swerve drive style drivetrain. */
public class Drivetrain {
public static final double kMaxSpeed = 3.0; // 3 meters per second
public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
@@ -25,19 +20,19 @@
private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
- private final SwerveModule m_frontLeft = new SwerveModule(1, 2);
- private final SwerveModule m_frontRight = new SwerveModule(3, 4);
- private final SwerveModule m_backLeft = new SwerveModule(5, 6);
- private final SwerveModule m_backRight = new SwerveModule(7, 8);
+ private final SwerveModule m_frontLeft = new SwerveModule(1, 2, 0, 1, 2, 3);
+ private final SwerveModule m_frontRight = new SwerveModule(3, 4, 4, 5, 6, 7);
+ private final SwerveModule m_backLeft = new SwerveModule(5, 6, 8, 9, 10, 11);
+ private final SwerveModule m_backRight = new SwerveModule(7, 8, 12, 13, 14, 15);
private final AnalogGyro m_gyro = new AnalogGyro(0);
- private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
- m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
- );
+ private final SwerveDriveKinematics m_kinematics =
+ new SwerveDriveKinematics(
+ m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
- private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
- m_gyro.getRotation2d());
+ private final SwerveDriveOdometry m_odometry =
+ new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
public Drivetrain() {
m_gyro.reset();
@@ -46,18 +41,18 @@
/**
* Method to drive the robot using joystick info.
*
- * @param xSpeed Speed of the robot in the x direction (forward).
- * @param ySpeed Speed of the robot in the y direction (sideways).
- * @param rot Angular rate of the robot.
+ * @param xSpeed Speed of the robot in the x direction (forward).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
*/
@SuppressWarnings("ParameterName")
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
- var swerveModuleStates = m_kinematics.toSwerveModuleStates(
- fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
- xSpeed, ySpeed, rot, m_gyro.getRotation2d())
- : new ChassisSpeeds(xSpeed, ySpeed, rot)
- );
+ var swerveModuleStates =
+ m_kinematics.toSwerveModuleStates(
+ fieldRelative
+ ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+ : new ChassisSpeeds(xSpeed, ySpeed, rot));
SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeed);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);
@@ -65,16 +60,13 @@
m_backRight.setDesiredState(swerveModuleStates[3]);
}
- /**
- * Updates the field relative position of the robot.
- */
+ /** Updates the field relative position of the robot. */
public void updateOdometry() {
m_odometry.update(
m_gyro.getRotation2d(),
m_frontLeft.getState(),
m_frontRight.getState(),
m_backLeft.getState(),
- m_backRight.getState()
- );
+ m_backRight.getState());
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Main.java
index 52400e6..31707aa 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.swervebot;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
index b8c4b8f..b2d39ef 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.swervebot;
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.SlewRateLimiter;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
@@ -36,23 +33,23 @@
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
final var xSpeed =
- -m_xspeedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
- * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed;
+ -m_xspeedLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftY(), 0.02))
+ * Drivetrain.kMaxSpeed;
// Get the y speed or sideways/strafe speed. We are inverting this because
// we want a positive value when we pull to the left. Xbox controllers
// return positive values when you pull to the right by default.
final var ySpeed =
- -m_yspeedLimiter.calculate(m_controller.getX(GenericHID.Hand.kLeft))
- * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed;
+ -m_yspeedLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftX(), 0.02))
+ * Drivetrain.kMaxSpeed;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
final var rot =
- -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
- * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxAngularSpeed;
+ -m_rotLimiter.calculate(MathUtil.applyDeadband(m_controller.getRightX(), 0.02))
+ * Drivetrain.kMaxAngularSpeed;
m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative);
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
index e13108f..9749c2d 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
@@ -1,55 +1,71 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.swervebot;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+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.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
public class SwerveModule {
private static final double kWheelRadius = 0.0508;
private static final int kEncoderResolution = 4096;
private static final double kModuleMaxAngularVelocity = Drivetrain.kMaxAngularSpeed;
- private static final double kModuleMaxAngularAcceleration
- = 2 * Math.PI; // radians per second squared
+ private static final double kModuleMaxAngularAcceleration =
+ 2 * Math.PI; // radians per second squared
- private final SpeedController m_driveMotor;
- private final SpeedController m_turningMotor;
+ private final MotorController m_driveMotor;
+ private final MotorController m_turningMotor;
- private final Encoder m_driveEncoder = new Encoder(0, 1);
- private final Encoder m_turningEncoder = new Encoder(2, 3);
+ private final Encoder m_driveEncoder;
+ private final Encoder m_turningEncoder;
+ // Gains are for example purposes only - must be determined for your own robot!
private final PIDController m_drivePIDController = new PIDController(1, 0, 0);
- private final ProfiledPIDController m_turningPIDController
- = new ProfiledPIDController(1, 0, 0,
- new TrapezoidProfile.Constraints(kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration));
+ // Gains are for example purposes only - must be determined for your own robot!
+ private final ProfiledPIDController m_turningPIDController =
+ new ProfiledPIDController(
+ 1,
+ 0,
+ 0,
+ new TrapezoidProfile.Constraints(
+ kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration));
// Gains are for example purposes only - must be determined for your own robot!
private final SimpleMotorFeedforward m_driveFeedforward = new SimpleMotorFeedforward(1, 3);
private final SimpleMotorFeedforward m_turnFeedforward = new SimpleMotorFeedforward(1, 0.5);
/**
- * Constructs a SwerveModule.
+ * Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning encoder.
*
- * @param driveMotorChannel ID for the drive motor.
- * @param turningMotorChannel ID for the turning motor.
+ * @param driveMotorChannel PWM output for the drive motor.
+ * @param turningMotorChannel PWM output for the turning motor.
+ * @param driveEncoderChannelA DIO input for the drive encoder channel A
+ * @param driveEncoderChannelB DIO input for the drive encoder channel B
+ * @param turningEncoderChannelA DIO input for the turning encoder channel A
+ * @param turningEncoderChannelB DIO input for the turning encoder channel B
*/
- public SwerveModule(int driveMotorChannel, int turningMotorChannel) {
- m_driveMotor = new PWMVictorSPX(driveMotorChannel);
- m_turningMotor = new PWMVictorSPX(turningMotorChannel);
+ public SwerveModule(
+ int driveMotorChannel,
+ int turningMotorChannel,
+ int driveEncoderChannelA,
+ int driveEncoderChannelB,
+ int turningEncoderChannelA,
+ int turningEncoderChannelB) {
+ m_driveMotor = new PWMSparkMax(driveMotorChannel);
+ m_turningMotor = new PWMSparkMax(turningMotorChannel);
+
+ m_driveEncoder = new Encoder(driveEncoderChannelA, driveEncoderChannelB);
+ m_turningEncoder = new Encoder(turningEncoderChannelA, turningEncoderChannelB);
// Set the distance per pulse for the drive encoder. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
@@ -57,8 +73,8 @@
m_driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
// Set the distance (in this case, angle) per pulse for the turning encoder.
- // This is the the angle through an entire rotation (2 * wpi::math::pi)
- // divided by the encoder resolution.
+ // This is the the angle through an entire rotation (2 * pi) divided by the
+ // encoder resolution.
m_turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution);
// Limit the PID Controller's input range between -pi and pi and set the input
@@ -78,19 +94,22 @@
/**
* Sets the desired state for the module.
*
- * @param state Desired state with speed and angle.
+ * @param desiredState Desired state with speed and angle.
*/
- public void setDesiredState(SwerveModuleState state) {
+ public void setDesiredState(SwerveModuleState desiredState) {
+ // Optimize the reference state to avoid spinning further than 90 degrees
+ SwerveModuleState state =
+ SwerveModuleState.optimize(desiredState, new Rotation2d(m_turningEncoder.get()));
+
// Calculate the drive output from the drive PID controller.
- final double driveOutput = m_drivePIDController.calculate(
- m_driveEncoder.getRate(), state.speedMetersPerSecond);
+ final double driveOutput =
+ m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
final double driveFeedforward = m_driveFeedforward.calculate(state.speedMetersPerSecond);
// Calculate the turning motor output from the turning PID controller.
- final double turnOutput = m_turningPIDController.calculate(
- m_turningEncoder.get(), state.angle.getRadians()
- );
+ final double turnOutput =
+ m_turningPIDController.calculate(m_turningEncoder.get(), state.angle.getRadians());
final double turnFeedforward =
m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java
index 706b15e..b4e8e54 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java
@@ -1,20 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.swervecontrollercommand;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
/**
* 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.
+ * 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.
@@ -31,37 +28,36 @@
public static final int kFrontRightTurningMotorPort = 5;
public static final int kRearRightTurningMotorPort = 7;
- public static final int[] kFrontLeftTurningEncoderPorts = new int[]{0, 1};
- public static final int[] kRearLeftTurningEncoderPorts = new int[]{2, 3};
- public static final int[] kFrontRightTurningEncoderPorts = new int[]{4, 5};
- public static final int[] kRearRightTurningEncoderPorts = new int[]{5, 6};
+ public static final int[] kFrontLeftTurningEncoderPorts = new int[] {0, 1};
+ public static final int[] kRearLeftTurningEncoderPorts = new int[] {2, 3};
+ public static final int[] kFrontRightTurningEncoderPorts = new int[] {4, 5};
+ public static final int[] kRearRightTurningEncoderPorts = new int[] {6, 7};
public static final boolean kFrontLeftTurningEncoderReversed = false;
public static final boolean kRearLeftTurningEncoderReversed = true;
public static final boolean kFrontRightTurningEncoderReversed = false;
public static final boolean kRearRightTurningEncoderReversed = true;
- public static final int[] kFrontLeftDriveEncoderPorts = new int[]{7, 8};
- public static final int[] kRearLeftDriveEncoderPorts = new int[]{9, 10};
- public static final int[] kFrontRightDriveEncoderPorts = new int[]{11, 12};
- public static final int[] kRearRightDriveEncoderPorts = new int[]{13, 14};
+ public static final int[] kFrontLeftDriveEncoderPorts = new int[] {8, 9};
+ public static final int[] kRearLeftDriveEncoderPorts = new int[] {10, 11};
+ public static final int[] kFrontRightDriveEncoderPorts = new int[] {12, 13};
+ public static final int[] kRearRightDriveEncoderPorts = new int[] {14, 15};
public static final boolean kFrontLeftDriveEncoderReversed = false;
public static final boolean kRearLeftDriveEncoderReversed = true;
public static final boolean kFrontRightDriveEncoderReversed = false;
public static final boolean kRearRightDriveEncoderReversed = true;
-
public static final double kTrackWidth = 0.5;
- //Distance between centers of right and left wheels on robot
+ // Distance between centers of right and left wheels on robot
public static final double kWheelBase = 0.7;
- //Distance between front and back wheels on robot
+ // Distance between front and back wheels on robot
public static final SwerveDriveKinematics kDriveKinematics =
new SwerveDriveKinematics(
- new Translation2d(kWheelBase / 2, kTrackWidth / 2),
- new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
- new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
- new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+ new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
public static final boolean kGyroReversed = false;
@@ -94,12 +90,10 @@
public static final double kPModuleTurningController = 1;
public static final double kPModuleDriveController = 1;
-
}
public static final class OIConstants {
- public static final int kDriverControllerPort = 1;
-
+ public static final int kDriverControllerPort = 0;
}
public static final class AutoConstants {
@@ -112,10 +106,9 @@
public static final double kPYController = 1;
public static final double kPThetaController = 1;
- //Constraint for the motion profilied robot angle controller
+ // Constraint for the motion profilied robot angle controller
public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
- new TrapezoidProfile.Constraints(kMaxAngularSpeedRadiansPerSecond,
- kMaxAngularSpeedRadiansPerSecondSquared);
-
+ new TrapezoidProfile.Constraints(
+ kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java
index 5c8a1da..af62a09 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.swervecontrollercommand;
@@ -15,8 +12,7 @@
* call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java
index 3d41ba4..800b70f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.swervecontrollercommand;
@@ -37,8 +34,8 @@
* 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.
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
@@ -49,20 +46,14 @@
CommandScheduler.getInstance().run();
}
- /**
- * This function is called once each time the robot enters Disabled mode.
- */
+ /** This function is called once each time the robot enters Disabled mode. */
@Override
- public void disabledInit() {
- }
+ public void disabledInit() {}
@Override
- public void disabledPeriodic() {
- }
+ public void disabledPeriodic() {}
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
}
}
- /**
- * This function is called periodically during autonomous.
- */
+ /** This function is called periodically during autonomous. */
@Override
- public void autonomousPeriodic() {
- }
+ public void autonomousPeriodic() {}
@Override
public void teleopInit() {
@@ -98,13 +86,9 @@
}
}
- /**
- * This function is called periodically during operator control.
- */
+ /** This function is called periodically during operator control. */
@Override
- public void teleopPeriodic() {
-
- }
+ public void teleopPeriodic() {}
@Override
public void testInit() {
@@ -112,10 +96,7 @@
CommandScheduler.getInstance().cancelAll();
}
- /**
- * This function is called periodically during test mode.
- */
+ /** This function is called periodically during test mode. */
@Override
- public void testPeriodic() {
- }
+ public void testPeriodic() {}
}
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 544cb1a..a1a299a 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
@@ -1,33 +1,27 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.swervecontrollercommand;
-import java.util.List;
-
-import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants;
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants;
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.RunCommand;
+import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import java.util.List;
/*
* This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -42,9 +36,7 @@
// The driver's controller
XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
- /**
- * The container for the robot. Contains subsystems, OI devices, and commands.
- */
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
@@ -54,22 +46,23 @@
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
- new RunCommand(() -> m_robotDrive.drive(
- m_driverController.getY(GenericHID.Hand.kLeft),
- m_driverController.getX(GenericHID.Hand.kRight),
- m_driverController.getX(GenericHID.Hand.kLeft), false)));
-
+ new RunCommand(
+ () ->
+ m_robotDrive.drive(
+ m_driverController.getLeftY(),
+ m_driverController.getRightX(),
+ m_driverController.getLeftX(),
+ false),
+ m_robotDrive));
}
/**
- * Use this method to define your button->command mappings. Buttons can be created by
- * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
* {@link JoystickButton}.
*/
- private void configureButtonBindings() {
- }
-
+ private void configureButtonBindings() {}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
@@ -79,43 +72,40 @@
public Command getAutonomousCommand() {
// Create config for trajectory
TrajectoryConfig config =
- new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
- AutoConstants.kMaxAccelerationMetersPerSecondSquared)
+ new TrajectoryConfig(
+ AutoConstants.kMaxSpeedMetersPerSecond,
+ AutoConstants.kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(DriveConstants.kDriveKinematics);
// An example trajectory to follow. All units in meters.
- Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
- // Start at the origin facing the +X direction
- new Pose2d(0, 0, new Rotation2d(0)),
- // Pass through these two interior waypoints, making an 's' curve path
- List.of(
- new Translation2d(1, 1),
- new Translation2d(2, -1)
- ),
- // End 3 meters straight ahead of where we started, facing forward
- new Pose2d(3, 0, new Rotation2d(0)),
- config
- );
+ Trajectory exampleTrajectory =
+ TrajectoryGenerator.generateTrajectory(
+ // Start at the origin facing the +X direction
+ new Pose2d(0, 0, new Rotation2d(0)),
+ // Pass through these two interior waypoints, making an 's' curve path
+ List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
+ // End 3 meters straight ahead of where we started, facing forward
+ new Pose2d(3, 0, new Rotation2d(0)),
+ config);
- var thetaController = new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0,
- AutoConstants.kThetaControllerConstraints);
+ var thetaController =
+ new ProfiledPIDController(
+ AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints);
thetaController.enableContinuousInput(-Math.PI, Math.PI);
- SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
- exampleTrajectory,
- m_robotDrive::getPose, //Functional interface to feed supplier
- DriveConstants.kDriveKinematics,
+ SwerveControllerCommand swerveControllerCommand =
+ new SwerveControllerCommand(
+ exampleTrajectory,
+ m_robotDrive::getPose, // Functional interface to feed supplier
+ DriveConstants.kDriveKinematics,
- //Position controllers
- new PIDController(AutoConstants.kPXController, 0, 0),
- new PIDController(AutoConstants.kPYController, 0, 0), thetaController,
-
- m_robotDrive::setModuleStates,
-
- m_robotDrive
-
- );
+ // Position controllers
+ new PIDController(AutoConstants.kPXController, 0, 0),
+ new PIDController(AutoConstants.kPYController, 0, 0),
+ thetaController,
+ m_robotDrive::setModuleStates,
+ m_robotDrive);
// Reset odometry to the starting pose of the trajectory.
m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
index 2baeec4..64d65b3 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
@@ -1,59 +1,56 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.swervecontrollercommand.subsystems;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants;
import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry;
-import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants;
-
-@SuppressWarnings("PMD.ExcessiveImports")
public class DriveSubsystem extends SubsystemBase {
- //Robot swerve modules
- private final SwerveModule m_frontLeft
- = new SwerveModule(DriveConstants.kFrontLeftDriveMotorPort,
- DriveConstants.kFrontLeftTurningMotorPort,
- DriveConstants.kFrontLeftDriveEncoderPorts,
- DriveConstants.kFrontLeftTurningEncoderPorts,
- DriveConstants.kFrontLeftDriveEncoderReversed,
- DriveConstants.kFrontLeftTurningEncoderReversed);
+ // Robot swerve modules
+ private final SwerveModule m_frontLeft =
+ new SwerveModule(
+ DriveConstants.kFrontLeftDriveMotorPort,
+ DriveConstants.kFrontLeftTurningMotorPort,
+ DriveConstants.kFrontLeftDriveEncoderPorts,
+ DriveConstants.kFrontLeftTurningEncoderPorts,
+ DriveConstants.kFrontLeftDriveEncoderReversed,
+ DriveConstants.kFrontLeftTurningEncoderReversed);
private final SwerveModule m_rearLeft =
- new SwerveModule(DriveConstants.kRearLeftDriveMotorPort,
- DriveConstants.kRearLeftTurningMotorPort,
- DriveConstants.kRearLeftDriveEncoderPorts,
- DriveConstants.kRearLeftTurningEncoderPorts,
- DriveConstants.kRearLeftDriveEncoderReversed,
- DriveConstants.kRearLeftTurningEncoderReversed);
-
+ new SwerveModule(
+ DriveConstants.kRearLeftDriveMotorPort,
+ DriveConstants.kRearLeftTurningMotorPort,
+ DriveConstants.kRearLeftDriveEncoderPorts,
+ DriveConstants.kRearLeftTurningEncoderPorts,
+ DriveConstants.kRearLeftDriveEncoderReversed,
+ DriveConstants.kRearLeftTurningEncoderReversed);
private final SwerveModule m_frontRight =
- new SwerveModule(DriveConstants.kFrontRightDriveMotorPort,
- DriveConstants.kFrontRightTurningMotorPort,
- DriveConstants.kFrontRightDriveEncoderPorts,
- DriveConstants.kFrontRightTurningEncoderPorts,
- DriveConstants.kFrontRightDriveEncoderReversed,
- DriveConstants.kFrontRightTurningEncoderReversed);
+ new SwerveModule(
+ DriveConstants.kFrontRightDriveMotorPort,
+ DriveConstants.kFrontRightTurningMotorPort,
+ DriveConstants.kFrontRightDriveEncoderPorts,
+ DriveConstants.kFrontRightTurningEncoderPorts,
+ DriveConstants.kFrontRightDriveEncoderReversed,
+ DriveConstants.kFrontRightTurningEncoderReversed);
private final SwerveModule m_rearRight =
- new SwerveModule(DriveConstants.kRearRightDriveMotorPort,
- DriveConstants.kRearRightTurningMotorPort,
- DriveConstants.kRearRightDriveEncoderPorts,
- DriveConstants.kRearRightTurningEncoderPorts,
- DriveConstants.kRearRightDriveEncoderReversed,
- DriveConstants.kRearRightTurningEncoderReversed);
+ new SwerveModule(
+ DriveConstants.kRearRightDriveMotorPort,
+ DriveConstants.kRearRightTurningMotorPort,
+ DriveConstants.kRearRightDriveEncoderPorts,
+ DriveConstants.kRearRightTurningEncoderPorts,
+ DriveConstants.kRearRightDriveEncoderReversed,
+ DriveConstants.kRearRightTurningEncoderReversed);
// The gyro sensor
private final Gyro m_gyro = new ADXRS450_Gyro();
@@ -62,17 +59,14 @@
SwerveDriveOdometry m_odometry =
new SwerveDriveOdometry(DriveConstants.kDriveKinematics, m_gyro.getRotation2d());
- /**
- * Creates a new DriveSubsystem.
- */
- public DriveSubsystem() {
- }
+ /** Creates a new DriveSubsystem. */
+ public DriveSubsystem() {}
@Override
public void periodic() {
// Update the odometry in the periodic block
m_odometry.update(
- new Rotation2d(getHeading()),
+ m_gyro.getRotation2d(),
m_frontLeft.getState(),
m_rearLeft.getState(),
m_frontRight.getState(),
@@ -100,20 +94,20 @@
/**
* Method to drive the robot using joystick info.
*
- * @param xSpeed Speed of the robot in the x direction (forward).
- * @param ySpeed Speed of the robot in the y direction (sideways).
- * @param rot Angular rate of the robot.
+ * @param xSpeed Speed of the robot in the x direction (forward).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
*/
@SuppressWarnings("ParameterName")
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
- var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
- fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
- xSpeed, ySpeed, rot, m_gyro.getRotation2d())
- : new ChassisSpeeds(xSpeed, ySpeed, rot)
- );
- SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates,
- DriveConstants.kMaxSpeedMetersPerSecond);
+ var swerveModuleStates =
+ DriveConstants.kDriveKinematics.toSwerveModuleStates(
+ fieldRelative
+ ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+ : new ChassisSpeeds(xSpeed, ySpeed, rot));
+ SwerveDriveKinematics.normalizeWheelSpeeds(
+ swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);
m_rearLeft.setDesiredState(swerveModuleStates[2]);
@@ -126,17 +120,15 @@
* @param desiredStates The desired SwerveModule states.
*/
public void setModuleStates(SwerveModuleState[] desiredStates) {
- SwerveDriveKinematics.normalizeWheelSpeeds(desiredStates,
- DriveConstants.kMaxSpeedMetersPerSecond);
+ SwerveDriveKinematics.normalizeWheelSpeeds(
+ desiredStates, DriveConstants.kMaxSpeedMetersPerSecond);
m_frontLeft.setDesiredState(desiredStates[0]);
m_frontRight.setDesiredState(desiredStates[1]);
m_rearLeft.setDesiredState(desiredStates[2]);
m_rearRight.setDesiredState(desiredStates[3]);
}
- /**
- * Resets the drive encoders to currently read a position of 0.
- */
+ /** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_frontLeft.resetEncoders();
m_rearLeft.resetEncoders();
@@ -144,9 +136,7 @@
m_rearRight.resetEncoders();
}
- /**
- * Zeroes the heading of the robot.
- */
+ /** Zeroes the heading of the robot. */
public void zeroHeading() {
m_gyro.reset();
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java
index e70c6ba..fd4c0b7 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java
@@ -1,21 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.swervecontrollercommand.subsystems;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+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.Spark;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
public class SwerveModule {
private final Spark m_driveMotor;
@@ -27,10 +23,12 @@
private final PIDController m_drivePIDController =
new PIDController(ModuleConstants.kPModuleDriveController, 0, 0);
- //Using a TrapezoidProfile PIDController to allow for smooth turning
- private final ProfiledPIDController m_turningPIDController
- = new ProfiledPIDController(
- ModuleConstants.kPModuleTurningController, 0, 0,
+ // Using a TrapezoidProfile PIDController to allow for smooth turning
+ private final ProfiledPIDController m_turningPIDController =
+ new ProfiledPIDController(
+ ModuleConstants.kPModuleTurningController,
+ 0,
+ 0,
new TrapezoidProfile.Constraints(
ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond,
ModuleConstants.kMaxModuleAngularAccelerationRadiansPerSecondSquared));
@@ -38,16 +36,16 @@
/**
* Constructs a SwerveModule.
*
- * @param driveMotorChannel ID for the drive motor.
+ * @param driveMotorChannel ID for the drive motor.
* @param turningMotorChannel ID for the turning motor.
*/
- public SwerveModule(int driveMotorChannel,
- int turningMotorChannel,
- int[] driveEncoderPorts,
- int[] turningEncoderPorts,
- boolean driveEncoderReversed,
- boolean turningEncoderReversed) {
-
+ public SwerveModule(
+ int driveMotorChannel,
+ int turningMotorChannel,
+ int[] driveEncoderPorts,
+ int[] turningEncoderPorts,
+ boolean driveEncoderReversed,
+ boolean turningEncoderReversed) {
m_driveMotor = new Spark(driveMotorChannel);
m_turningMotor = new Spark(turningMotorChannel);
@@ -60,15 +58,15 @@
// resolution.
m_driveEncoder.setDistancePerPulse(ModuleConstants.kDriveEncoderDistancePerPulse);
- //Set whether drive encoder should be reversed or not
+ // Set whether drive encoder should be reversed or not
m_driveEncoder.setReverseDirection(driveEncoderReversed);
// Set the distance (in this case, angle) per pulse for the turning encoder.
- // This is the the angle through an entire rotation (2 * wpi::math::pi)
- // divided by the encoder resolution.
+ // This is the the angle through an entire rotation (2 * pi) divided by the
+ // encoder resolution.
m_turningEncoder.setDistancePerPulse(ModuleConstants.kTurningEncoderDistancePerPulse);
- //Set whether turning encoder should be reversed or not
+ // Set whether turning encoder should be reversed or not
m_turningEncoder.setReverseDirection(turningEncoderReversed);
// Limit the PID Controller's input range between -pi and pi and set the input
@@ -88,27 +86,27 @@
/**
* Sets the desired state for the module.
*
- * @param state Desired state with speed and angle.
+ * @param desiredState Desired state with speed and angle.
*/
- public void setDesiredState(SwerveModuleState state) {
+ public void setDesiredState(SwerveModuleState desiredState) {
+ // Optimize the reference state to avoid spinning further than 90 degrees
+ SwerveModuleState state =
+ SwerveModuleState.optimize(desiredState, new Rotation2d(m_turningEncoder.get()));
+
// Calculate the drive output from the drive PID controller.
- final var driveOutput = m_drivePIDController.calculate(
- m_driveEncoder.getRate(), state.speedMetersPerSecond);
+ final double driveOutput =
+ m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
// Calculate the turning motor output from the turning PID controller.
- final var turnOutput = m_turningPIDController.calculate(
- m_turningEncoder.get(), state.angle.getRadians()
- );
+ final var turnOutput =
+ m_turningPIDController.calculate(m_turningEncoder.get(), state.angle.getRadians());
// Calculate the turning motor output from the turning PID controller.
m_driveMotor.set(driveOutput);
m_turningMotor.set(turnOutput);
}
- /**
- * Zeros all the SwerveModule encoders.
- */
-
+ /** Zeros all the SwerveModule encoders. */
public void resetEncoders() {
m_driveEncoder.reset();
m_turningEncoder.reset();
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
new file mode 100644
index 0000000..0e8feef
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
@@ -0,0 +1,91 @@
+// 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.swervedriveposeestimator;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Timer;
+
+/** Represents a swerve drive style drivetrain. */
+public class Drivetrain {
+ public static final double kMaxSpeed = 3.0; // 3 meters per second
+ public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
+
+ private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
+ private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
+ private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
+ private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
+
+ private final SwerveModule m_frontLeft = new SwerveModule(1, 2, 0, 1, 2, 3);
+ private final SwerveModule m_frontRight = new SwerveModule(3, 4, 4, 5, 6, 7);
+ private final SwerveModule m_backLeft = new SwerveModule(5, 6, 8, 9, 10, 11);
+ private final SwerveModule m_backRight = new SwerveModule(7, 8, 12, 13, 14, 15);
+
+ private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+ private final SwerveDriveKinematics m_kinematics =
+ new SwerveDriveKinematics(
+ m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
+
+ /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
+ below are robot specific, and should be tuned. */
+ private final SwerveDrivePoseEstimator m_poseEstimator =
+ new SwerveDrivePoseEstimator(
+ m_gyro.getRotation2d(),
+ new Pose2d(),
+ m_kinematics,
+ VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
+ VecBuilder.fill(Units.degreesToRadians(0.01)),
+ VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+
+ public Drivetrain() {
+ m_gyro.reset();
+ }
+
+ /**
+ * Method to drive the robot using joystick info.
+ *
+ * @param xSpeed Speed of the robot in the x direction (forward).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
+ * @param fieldRelative Whether the provided x and y speeds are relative to the field.
+ */
+ @SuppressWarnings("ParameterName")
+ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+ var swerveModuleStates =
+ m_kinematics.toSwerveModuleStates(
+ fieldRelative
+ ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+ : new ChassisSpeeds(xSpeed, ySpeed, rot));
+ SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeed);
+ m_frontLeft.setDesiredState(swerveModuleStates[0]);
+ m_frontRight.setDesiredState(swerveModuleStates[1]);
+ m_backLeft.setDesiredState(swerveModuleStates[2]);
+ m_backRight.setDesiredState(swerveModuleStates[3]);
+ }
+
+ /** Updates the field relative position of the robot. */
+ public void updateOdometry() {
+ m_poseEstimator.update(
+ m_gyro.getRotation2d(),
+ m_frontLeft.getState(),
+ m_frontRight.getState(),
+ m_backLeft.getState(),
+ m_backRight.getState());
+
+ // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
+ // a real robot, this must be calculated based either on latency or timestamps.
+ m_poseEstimator.addVisionMeasurement(
+ ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
+ m_poseEstimator.getEstimatedPosition()),
+ Timer.getFPGATimestamp() - 0.3);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/ExampleGlobalMeasurementSensor.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/ExampleGlobalMeasurementSensor.java
new file mode 100644
index 0000000..a92a4fe
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/ExampleGlobalMeasurementSensor.java
@@ -0,0 +1,32 @@
+// 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.swervedriveposeestimator;
+
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+
+/** This dummy class represents a global measurement sensor, such as a computer vision solution. */
+public final class ExampleGlobalMeasurementSensor {
+ private ExampleGlobalMeasurementSensor() {
+ // Utility class
+ }
+
+ /**
+ * Get a "noisy" fake global pose reading.
+ *
+ * @param estimatedRobotPose The robot pose.
+ */
+ public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
+ var rand =
+ StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+ return new Pose2d(
+ estimatedRobotPose.getX() + rand.get(0, 0),
+ estimatedRobotPose.getY() + rand.get(1, 0),
+ estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0))));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Main.java
new file mode 100644
index 0000000..2221f14
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/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.swervedriveposeestimator;
+
+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/swervedriveposeestimator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Robot.java
new file mode 100644
index 0000000..7fbee8d
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Robot.java
@@ -0,0 +1,49 @@
+// 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.swervedriveposeestimator;
+
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class Robot extends TimedRobot {
+ private final XboxController m_controller = new XboxController(0);
+ private final Drivetrain m_swerve = new Drivetrain();
+
+ // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+ private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3);
+ private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3);
+ private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
+ @Override
+ public void autonomousPeriodic() {
+ driveWithJoystick(false);
+ m_swerve.updateOdometry();
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ driveWithJoystick(true);
+ }
+
+ private void driveWithJoystick(boolean fieldRelative) {
+ // Get the x speed. We are inverting this because Xbox controllers return
+ // negative values when we push forward.
+ final var xSpeed = -m_xspeedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
+
+ // Get the y speed or sideways/strafe speed. We are inverting this because
+ // we want a positive value when we pull to the left. Xbox controllers
+ // return positive values when you pull to the right by default.
+ final var ySpeed = -m_yspeedLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxSpeed;
+
+ // Get the rate of angular rotation. We are inverting this because we want a
+ // positive value when we pull to the left (remember, CCW is positive in
+ // mathematics). Xbox controllers return positive values when you pull to
+ // the right by default.
+ final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
+
+ m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative);
+ }
+}
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
new file mode 100644
index 0000000..d7c43dd
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
@@ -0,0 +1,120 @@
+// 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.swervedriveposeestimator;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+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 {
+ private static final double kWheelRadius = 0.0508;
+ private static final int kEncoderResolution = 4096;
+
+ private static final double kModuleMaxAngularVelocity = Drivetrain.kMaxAngularSpeed;
+ private static final double kModuleMaxAngularAcceleration =
+ 2 * Math.PI; // radians per second squared
+
+ private final MotorController m_driveMotor;
+ private final MotorController m_turningMotor;
+
+ private final Encoder m_driveEncoder;
+ private final Encoder m_turningEncoder;
+
+ // Gains are for example purposes only - must be determined for your own robot!
+ private final PIDController m_drivePIDController = new PIDController(1, 0, 0);
+
+ // Gains are for example purposes only - must be determined for your own robot!
+ private final ProfiledPIDController m_turningPIDController =
+ new ProfiledPIDController(
+ 1,
+ 0,
+ 0,
+ new TrapezoidProfile.Constraints(
+ kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration));
+
+ // Gains are for example purposes only - must be determined for your own robot!
+ private final SimpleMotorFeedforward m_driveFeedforward = new SimpleMotorFeedforward(1, 3);
+ private final SimpleMotorFeedforward m_turnFeedforward = new SimpleMotorFeedforward(1, 0.5);
+
+ /**
+ * Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning encoder.
+ *
+ * @param driveMotorChannel PWM output for the drive motor.
+ * @param turningMotorChannel PWM output for the turning motor.
+ * @param driveEncoderChannelA DIO input for the drive encoder channel A
+ * @param driveEncoderChannelB DIO input for the drive encoder channel B
+ * @param turningEncoderChannelA DIO input for the turning encoder channel A
+ * @param turningEncoderChannelB DIO input for the turning encoder channel B
+ */
+ public SwerveModule(
+ int driveMotorChannel,
+ int turningMotorChannel,
+ int driveEncoderChannelA,
+ int driveEncoderChannelB,
+ int turningEncoderChannelA,
+ int turningEncoderChannelB) {
+ m_driveMotor = new PWMSparkMax(driveMotorChannel);
+ m_turningMotor = new PWMSparkMax(turningMotorChannel);
+
+ m_driveEncoder = new Encoder(driveEncoderChannelA, driveEncoderChannelB);
+ m_turningEncoder = new Encoder(turningEncoderChannelA, turningEncoderChannelB);
+
+ // Set the distance per pulse for the drive encoder. We can simply use the
+ // distance traveled for one rotation of the wheel divided by the encoder
+ // resolution.
+ m_driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+
+ // Set the distance (in this case, angle) per pulse for the turning encoder.
+ // This is the the angle through an entire rotation (2 * pi) divided by the
+ // encoder resolution.
+ m_turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution);
+
+ // Limit the PID Controller's input range between -pi and pi and set the input
+ // to be continuous.
+ m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
+ }
+
+ /**
+ * Returns the current state of the module.
+ *
+ * @return The current state of the module.
+ */
+ public SwerveModuleState getState() {
+ return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get()));
+ }
+
+ /**
+ * Sets the desired state for the module.
+ *
+ * @param desiredState Desired state with speed and angle.
+ */
+ public void setDesiredState(SwerveModuleState desiredState) {
+ // Optimize the reference state to avoid spinning further than 90 degrees
+ SwerveModuleState state =
+ SwerveModuleState.optimize(desiredState, new Rotation2d(m_turningEncoder.get()));
+
+ // Calculate the drive output from the drive PID controller.
+ final double driveOutput =
+ m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
+
+ final double driveFeedforward = m_driveFeedforward.calculate(state.speedMetersPerSecond);
+
+ // Calculate the turning motor output from the turning PID controller.
+ final double turnOutput =
+ m_turningPIDController.calculate(m_turningEncoder.get(), state.angle.getRadians());
+
+ final double turnFeedforward =
+ m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
+
+ m_driveMotor.setVoltage(driveOutput + driveFeedforward);
+ m_turningMotor.setVoltage(turnOutput + turnFeedforward);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Main.java
index e1dc017..404d96b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.tankdrive;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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
old mode 100755
new mode 100644
index b8a9202..c91767a
--- 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
@@ -1,29 +1,35 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.tankdrive;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
- * This is a demo program showing the use of the RobotDrive class, specifically
- * it contains the code necessary to operate a robot with tank drive.
+ * This is a demo program showing the use of the DifferentialDrive class, specifically it contains
+ * the code necessary to operate a robot with tank drive.
*/
public class Robot extends TimedRobot {
private DifferentialDrive m_myRobot;
private Joystick m_leftStick;
private Joystick m_rightStick;
+ private final MotorController m_leftMotor = new PWMSparkMax(0);
+ private final MotorController m_rightMotor = new PWMSparkMax(1);
+
@Override
public void robotInit() {
- m_myRobot = new DifferentialDrive(new PWMVictorSPX(0), new PWMVictorSPX(1));
+ // 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_leftStick = new Joystick(0);
m_rightStick = new Joystick(1);
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Main.java
index 3adcee9..ada70f2 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.tankdrivexboxcontroller;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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 f0a9e76..f05e22c 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
@@ -1,35 +1,38 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.tankdrivexboxcontroller;
-import edu.wpi.first.wpilibj.GenericHID.Hand;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
- * This is a demo program showing the use of the DifferentialDrive class.
- * Runs the motors with tank steering and an Xbox controller.
+ * This is a demo program showing the use of the DifferentialDrive class. Runs the motors with tank
+ * steering and an Xbox controller.
*/
public class Robot extends TimedRobot {
- private final PWMVictorSPX m_leftMotor = new PWMVictorSPX(0);
- private final PWMVictorSPX m_rightMotor = new PWMVictorSPX(1);
+ 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 XboxController m_driverController = new XboxController(0);
@Override
+ public void robotInit() {
+ // 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);
+ }
+
+ @Override
public void teleopPeriodic() {
// Drive with tank drive.
// That means that the Y axis of the left stick moves the left side
// of the robot forward and backward, and the Y axis of the right stick
// moves the right side of the robot forward and backward.
- m_robotDrive.tankDrive(m_driverController.getY(Hand.kLeft),
- m_driverController.getY(Hand.kRight));
+ m_robotDrive.tankDrive(m_driverController.getLeftY(), m_driverController.getRightY());
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Main.java
index edf56f8..3f4de06 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.ultrasonic;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
index 2cae288..199d6f6 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
@@ -1,23 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.ultrasonic;
+import edu.wpi.first.math.filter.MedianFilter;
import edu.wpi.first.wpilibj.AnalogInput;
-import edu.wpi.first.wpilibj.MedianFilter;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
- * This is a sample program demonstrating how to use an ultrasonic sensor and
- * proportional control to maintain a set distance from an object.
+ * This is a sample program demonstrating how to use an ultrasonic sensor and proportional control
+ * to maintain a set distance from an object.
*/
-
public class Robot extends TimedRobot {
// distance in inches the robot wants to stay from an object
private static final double kHoldDistance = 12.0;
@@ -36,13 +32,12 @@
private final MedianFilter m_filter = new MedianFilter(10);
private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
- private final DifferentialDrive m_robotDrive
- = new DifferentialDrive(new PWMVictorSPX(kLeftMotorPort),
- new PWMVictorSPX(kRightMotorPort));
+ private final DifferentialDrive m_robotDrive =
+ new DifferentialDrive(new PWMSparkMax(kLeftMotorPort), new PWMSparkMax(kRightMotorPort));
/**
- * Tells the robot to drive to a set distance (in inches) from an object
- * using proportional control.
+ * Tells the robot to drive to a set distance (in inches) from an object using proportional
+ * control.
*/
@Override
public void teleopPeriodic() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Main.java
index b8bffc7..b31e376 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.ultrasonicpid;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
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 b023429..526ac53 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
@@ -1,22 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.ultrasonicpid;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.filter.MedianFilter;
import edu.wpi.first.wpilibj.AnalogInput;
-import edu.wpi.first.wpilibj.MedianFilter;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
- * This is a sample program to demonstrate the use of a PIDController with an
- * ultrasonic sensor to reach and maintain a set distance from an object.
+ * This is a sample program to demonstrate the use of a PIDController with an ultrasonic sensor to
+ * reach and maintain a set distance from an object.
*/
public class Robot extends TimedRobot {
// distance in inches the robot wants to stay from an object
@@ -42,9 +39,8 @@
private final MedianFilter m_filter = new MedianFilter(5);
private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
- private final DifferentialDrive m_robotDrive
- = new DifferentialDrive(new PWMVictorSPX(kLeftMotorPort),
- new PWMVictorSPX(kRightMotorPort));
+ private final DifferentialDrive m_robotDrive =
+ new DifferentialDrive(new PWMSparkMax(kLeftMotorPort), new PWMSparkMax(kRightMotorPort));
private final PIDController m_pidController = new PIDController(kP, kI, kD);
@Override
@@ -57,8 +53,7 @@
public void teleopPeriodic() {
// returned value is filtered with a rolling median filter, since ultrasonics
// tend to be quite noisy and susceptible to sudden outliers
- double pidOutput
- = m_pidController.calculate(m_filter.calculate(m_ultrasonic.getVoltage()));
+ double pidOutput = m_pidController.calculate(m_filter.calculate(m_ultrasonic.getVoltage()));
m_robotDrive.arcadeDrive(pidOutput, 0);
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Constants.java
index 0184096..ecfe201 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Constants.java
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.templates.commandbased;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants. This class should not be used for any other purpose. All constants should be
- * declared globally (i.e. public static). Do not put anything functional in this class.
+ * 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 final class Constants {}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java
index b529f34..7fa438b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.templates.commandbased;
@@ -15,8 +12,7 @@
* call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java
index c0bc795..34cd3ce 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.templates.commandbased;
@@ -37,8 +34,8 @@
* 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.
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
@@ -49,20 +46,14 @@
CommandScheduler.getInstance().run();
}
- /**
- * This function is called once each time the robot enters Disabled mode.
- */
+ /** This function is called once each time the robot enters Disabled mode. */
@Override
- public void disabledInit() {
- }
+ public void disabledInit() {}
@Override
- public void disabledPeriodic() {
- }
+ public void disabledPeriodic() {}
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -73,12 +64,9 @@
}
}
- /**
- * This function is called periodically during autonomous.
- */
+ /** This function is called periodically during autonomous. */
@Override
- public void autonomousPeriodic() {
- }
+ public void autonomousPeriodic() {}
@Override
public void teleopInit() {
@@ -91,12 +79,9 @@
}
}
- /**
- * This function is called periodically during operator control.
- */
+ /** This function is called periodically during operator control. */
@Override
- public void teleopPeriodic() {
- }
+ public void teleopPeriodic() {}
@Override
public void testInit() {
@@ -104,10 +89,7 @@
CommandScheduler.getInstance().cancelAll();
}
- /**
- * This function is called periodically during test mode.
- */
+ /** This function is called periodically during test mode. */
@Override
- public void testPeriodic() {
- }
+ public void testPeriodic() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotContainer.java
index 4592f2e..75f79ae 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotContainer.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.templates.commandbased;
@@ -14,10 +11,10 @@
import edu.wpi.first.wpilibj2.command.Command;
/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * 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.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems and commands are defined here...
@@ -25,25 +22,19 @@
private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem);
-
-
- /**
- * The container for the robot. Contains subsystems, OI devices, and commands.
- */
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
}
/**
- * Use this method to define your button->command mappings. Buttons can be created by
+ * Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
- * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
- * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+ * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
- private void configureButtonBindings() {
- }
-
+ private void configureButtonBindings() {}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java
index 7cfe642..194cbf6 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java
@@ -1,18 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.templates.commandbased.commands;
import edu.wpi.first.wpilibj.templates.commandbased.subsystems.ExampleSubsystem;
import edu.wpi.first.wpilibj2.command.CommandBase;
-/**
- * An example command that uses an example subsystem.
- */
+/** An example command that uses an example subsystem. */
public class ExampleCommand extends CommandBase {
@SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
private final ExampleSubsystem m_subsystem;
@@ -30,18 +25,15 @@
// Called when the command is initially scheduled.
@Override
- public void initialize() {
- }
+ public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
- public void execute() {
- }
+ public void execute() {}
// Called once the command ends or is interrupted.
@Override
- public void end(boolean interrupted) {
- }
+ public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
index 76202fe..8a3594b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
@@ -1,21 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.templates.commandbased.subsystems;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class ExampleSubsystem extends SubsystemBase {
- /**
- * Creates a new ExampleSubsystem.
- */
- public ExampleSubsystem() {
-
- }
+ /** Creates a new ExampleSubsystem. */
+ public ExampleSubsystem() {}
@Override
public void periodic() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/EducationalRobot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/EducationalRobot.java
new file mode 100644
index 0000000..76d13a4
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/EducationalRobot.java
@@ -0,0 +1,77 @@
+// 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.templates.educational;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
+
+/** Educational robot base class. */
+public class EducationalRobot extends RobotBase {
+ public void robotInit() {}
+
+ public void disabled() {}
+
+ public void run() {}
+
+ public void autonomous() {
+ run();
+ }
+
+ public void teleop() {
+ run();
+ }
+
+ public void test() {
+ run();
+ }
+
+ private volatile boolean m_exit;
+
+ @Override
+ public void startCompetition() {
+ robotInit();
+
+ // Tell the DS that the robot is ready to be enabled
+ HAL.observeUserProgramStarting();
+
+ while (!Thread.currentThread().isInterrupted() && !m_exit) {
+ if (isDisabled()) {
+ DriverStation.inDisabled(true);
+ disabled();
+ DriverStation.inDisabled(false);
+ while (isDisabled()) {
+ DriverStation.waitForData();
+ }
+ } else if (isAutonomous()) {
+ DriverStation.inAutonomous(true);
+ autonomous();
+ DriverStation.inAutonomous(false);
+ while (isAutonomousEnabled()) {
+ DriverStation.waitForData();
+ }
+ } else if (isTest()) {
+ DriverStation.inTest(true);
+ test();
+ DriverStation.inTest(false);
+ while (isTest() && isEnabled()) {
+ DriverStation.waitForData();
+ }
+ } else {
+ DriverStation.inTeleop(true);
+ teleop();
+ DriverStation.inTeleop(false);
+ while (isTeleopEnabled()) {
+ DriverStation.waitForData();
+ }
+ }
+ }
+ }
+
+ @Override
+ public void endCompetition() {
+ m_exit = true;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/Main.java
new file mode 100644
index 0000000..80b9bc9
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/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.templates.educational;
+
+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/templates/educational/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/Robot.java
new file mode 100644
index 0000000..0643977
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/Robot.java
@@ -0,0 +1,23 @@
+// 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.templates.educational;
+
+/**
+ * The VM is configured to automatically run this class, and to call the run() function when the
+ * robot is enabled. 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 EducationalRobot {
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {}
+
+ /** This function is run when the robot is enabled. */
+ @Override
+ public void run() {}
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Main.java
deleted file mode 100644
index 67d8377..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Main.java
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.oldcommandbased;
-
-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/templates/oldcommandbased/OI.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/OI.java
deleted file mode 100644
index d4c7f91..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/OI.java
+++ /dev/null
@@ -1,42 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.oldcommandbased;
-
-/**
- * This class is the glue that binds the controls on the physical operator
- * interface to the commands and command groups that allow control of the robot.
- */
-public class OI {
- //// CREATING BUTTONS
- // One type of button is a joystick button which is any button on a
- //// joystick.
- // You create one by telling it which joystick it's on and which button
- // number it is.
- // Joystick stick = new Joystick(port);
- // Button button = new JoystickButton(stick, buttonNumber);
-
- // There are a few additional built in buttons you can use. Additionally,
- // by subclassing Button you can create custom triggers and bind those to
- // commands the same as any other Button.
-
- //// TRIGGERING COMMANDS WITH BUTTONS
- // Once you have a button, it's trivial to bind it to a button in one of
- // three ways:
-
- // Start the command when the button is pressed and let it run the command
- // until it is finished as determined by it's isFinished method.
- // button.whenPressed(new ExampleCommand());
-
- // Run the command while the button is being held down and interrupt it once
- // the button is released.
- // button.whileHeld(new ExampleCommand());
-
- // Start the command when the button is released and let it run the command
- // until it is finished as determined by it's isFinished method.
- // button.whenReleased(new ExampleCommand());
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Robot.java
deleted file mode 100644
index 8f000ac..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Robot.java
+++ /dev/null
@@ -1,131 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.oldcommandbased;
-
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.command.Scheduler;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.templates.oldcommandbased.commands.ExampleCommand;
-import edu.wpi.first.wpilibj.templates.oldcommandbased.subsystems.ExampleSubsystem;
-
-/**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the TimedRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the build.gradle file in the
- * project.
- */
-public class Robot extends TimedRobot {
- public static ExampleSubsystem m_subsystem = new ExampleSubsystem();
- public static OI m_oi;
-
- Command m_autonomousCommand;
- SendableChooser<Command> m_chooser = new SendableChooser<>();
-
- /**
- * This function is run when the robot is first started up and should be
- * used for any initialization code.
- */
- @Override
- public void robotInit() {
- m_oi = new OI();
- m_chooser.setDefaultOption("Default Auto", new ExampleCommand());
- // chooser.addOption("My Auto", new MyAutoCommand());
- SmartDashboard.putData("Auto mode", m_chooser);
- }
-
- /**
- * 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() {
- }
-
- /**
- * This function is called once each time the robot enters Disabled mode.
- * You can use it to reset any subsystem information you want to clear when
- * the robot is disabled.
- */
- @Override
- public void disabledInit() {
- }
-
- @Override
- public void disabledPeriodic() {
- Scheduler.getInstance().run();
- }
-
- /**
- * This autonomous (along with the chooser code above) shows how to select
- * between different autonomous modes using the dashboard. The sendable
- * chooser code works with the Java SmartDashboard. If you prefer the
- * LabVIEW Dashboard, remove all of the chooser code and uncomment the
- * getString code to get the auto name from the text box below the Gyro
- *
- * <p>You can add additional auto modes by adding additional commands to the
- * chooser code above (like the commented example) or additional comparisons
- * to the switch structure below with additional strings & commands.
- */
- @Override
- public void autonomousInit() {
- m_autonomousCommand = m_chooser.getSelected();
-
- /*
- * String autoSelected = SmartDashboard.getString("Auto Selector",
- * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
- * = new MyAutoCommand(); break; case "Default Auto": default:
- * autonomousCommand = new ExampleCommand(); break; }
- */
-
- // schedule the autonomous command (example)
- if (m_autonomousCommand != null) {
- m_autonomousCommand.start();
- }
- }
-
- /**
- * This function is called periodically during autonomous.
- */
- @Override
- public void autonomousPeriodic() {
- Scheduler.getInstance().run();
- }
-
- @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() {
- Scheduler.getInstance().run();
- }
-
- /**
- * This function is called periodically during test mode.
- */
- @Override
- public void testPeriodic() {
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/RobotMap.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/RobotMap.java
deleted file mode 100644
index 9f33b02..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/RobotMap.java
+++ /dev/null
@@ -1,26 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.oldcommandbased;
-
-/**
- * The RobotMap is a mapping from the ports sensors and actuators are wired into
- * to a variable name. This provides flexibility changing wiring, makes checking
- * the wiring easier and significantly reduces the number of magic numbers
- * floating around.
- */
-public class RobotMap {
- // For example to map the left and right motors, you could define the
- // following variables to use with your drivetrain subsystem.
- // public static int leftMotor = 1;
- // public static int rightMotor = 2;
-
- // If you are using multiple modules, make sure to define both the port
- // number and the module. For example you with a rangefinder:
- // public static int rangefinderPort = 1;
- // public static int rangefinderModule = 1;
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/commands/ExampleCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/commands/ExampleCommand.java
deleted file mode 100644
index 0d84987..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/commands/ExampleCommand.java
+++ /dev/null
@@ -1,48 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.oldcommandbased.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.templates.oldcommandbased.Robot;
-
-/**
- * An example command. You can replace me with your own command.
- */
-public class ExampleCommand extends Command {
- public ExampleCommand() {
- // Use requires() here to declare subsystem dependencies
- requires(Robot.m_subsystem);
- }
-
- // Called just before this Command runs the first time
- @Override
- protected void initialize() {
- }
-
- // Called repeatedly when this Command is scheduled to run
- @Override
- protected void execute() {
- }
-
- // Make this return true when this Command no longer needs to run execute()
- @Override
- protected boolean isFinished() {
- return false;
- }
-
- // Called once after isFinished returns true
- @Override
- protected void end() {
- }
-
- // Called when another command which requires one or more of the same
- // subsystems is scheduled to run
- @Override
- protected void interrupted() {
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/subsystems/ExampleSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/subsystems/ExampleSubsystem.java
deleted file mode 100644
index f3d1de9..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/subsystems/ExampleSubsystem.java
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.oldcommandbased.subsystems;
-
-import edu.wpi.first.wpilibj.command.Subsystem;
-
-/**
- * An example subsystem. You can replace with me with your own subsystem.
- */
-public class ExampleSubsystem extends Subsystem {
- // Put methods for controlling this subsystem
- // here. Call these from Commands.
-
-
- @Override
- protected void initDefaultCommand() {
- // Set the default command for a subsystem here.
- // setDefaultCommand(new MySpecialCommand());
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Main.java
index 65c6268..e465d47 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.templates.robotbaseskeleton;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
index bfd3c46..787395c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
@@ -1,41 +1,32 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.templates.robotbaseskeleton;
import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
/**
- * The VM is configured to automatically run this class. If you change the name
- * of this class or the package after creating this project, you must also
- * update the build.gradle file in the project.
+ * The VM is configured to automatically run this class. If you change the name of this class or the
+ * package after creating this project, you must also update the build.gradle file in the project.
*/
public class Robot extends RobotBase {
- public void robotInit() {
- }
+ public void robotInit() {}
- public void disabled() {
- }
+ public void disabled() {}
- public void autonomous() {
- }
+ public void autonomous() {}
- public void teleop() {
- }
+ public void teleop() {}
- public void test() {
- }
+ public void test() {}
private volatile boolean m_exit;
- @SuppressWarnings("PMD.CyclomaticComplexity")
@Override
public void startCompetition() {
robotInit();
@@ -45,36 +36,36 @@
while (!Thread.currentThread().isInterrupted() && !m_exit) {
if (isDisabled()) {
- m_ds.InDisabled(true);
+ DriverStation.inDisabled(true);
disabled();
- m_ds.InDisabled(false);
+ DriverStation.inDisabled(false);
while (isDisabled()) {
- m_ds.waitForData();
+ DriverStation.waitForData();
}
} else if (isAutonomous()) {
- m_ds.InAutonomous(true);
+ DriverStation.inAutonomous(true);
autonomous();
- m_ds.InAutonomous(false);
+ DriverStation.inAutonomous(false);
while (isAutonomousEnabled()) {
- m_ds.waitForData();
+ DriverStation.waitForData();
}
} else if (isTest()) {
LiveWindow.setEnabled(true);
Shuffleboard.enableActuatorWidgets();
- m_ds.InTest(true);
+ DriverStation.inTest(true);
test();
- m_ds.InTest(false);
+ DriverStation.inTest(false);
while (isTest() && isEnabled()) {
- m_ds.waitForData();
+ DriverStation.waitForData();
}
LiveWindow.setEnabled(false);
Shuffleboard.disableActuatorWidgets();
} else {
- m_ds.InOperatorControl(true);
+ DriverStation.inTeleop(true);
teleop();
- m_ds.InOperatorControl(false);
- while (isOperatorControlEnabled()) {
- m_ds.waitForData();
+ DriverStation.inTeleop(false);
+ while (isTeleopEnabled()) {
+ DriverStation.waitForData();
}
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/Constants.java
new file mode 100644
index 0000000..9a1ab69
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/Constants.java
@@ -0,0 +1,15 @@
+// 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.templates.romicommandbased;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/Main.java
new file mode 100644
index 0000000..b73f963
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/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.templates.romicommandbased;
+
+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/templates/romicommandbased/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/Robot.java
new file mode 100644
index 0000000..2475ee6
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/Robot.java
@@ -0,0 +1,95 @@
+// 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.templates.romicommandbased;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /** This function is called once each time the robot enters Disabled mode. */
+ @Override
+ public void disabledInit() {}
+
+ @Override
+ public void disabledPeriodic() {}
+
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /** This function is called periodically during autonomous. */
+ @Override
+ public void autonomousPeriodic() {}
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /** This function is called periodically during operator control. */
+ @Override
+ public void teleopPeriodic() {}
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /** This function is called periodically during test mode. */
+ @Override
+ public void testPeriodic() {}
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/RobotContainer.java
new file mode 100644
index 0000000..11596a5
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/RobotContainer.java
@@ -0,0 +1,47 @@
+// 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.templates.romicommandbased;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.templates.romicommandbased.commands.ExampleCommand;
+import edu.wpi.first.wpilibj.templates.romicommandbased.subsystems.RomiDrivetrain;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems and commands are defined here...
+ private final RomiDrivetrain m_romiDrivetrain = new RomiDrivetrain();
+
+ private final ExampleCommand m_autoCommand = new ExampleCommand(m_romiDrivetrain);
+
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+ * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ */
+ private void configureButtonBindings() {}
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ // An ExampleCommand will run in autonomous
+ return m_autoCommand;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/commands/ExampleCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/commands/ExampleCommand.java
new file mode 100644
index 0000000..0ebb5d2
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/commands/ExampleCommand.java
@@ -0,0 +1,43 @@
+// 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.templates.romicommandbased.commands;
+
+import edu.wpi.first.wpilibj.templates.romicommandbased.subsystems.RomiDrivetrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/** An example command that uses an example subsystem. */
+public class ExampleCommand extends CommandBase {
+ @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+ private final RomiDrivetrain m_subsystem;
+
+ /**
+ * Creates a new ExampleCommand.
+ *
+ * @param subsystem The subsystem used by this command.
+ */
+ public ExampleCommand(RomiDrivetrain subsystem) {
+ m_subsystem = subsystem;
+ // Use addRequirements() here to declare subsystem dependencies.
+ addRequirements(subsystem);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {}
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {}
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {}
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
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
new file mode 100644
index 0000000..80543c6
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/subsystems/RomiDrivetrain.java
@@ -0,0 +1,63 @@
+// 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.templates.romicommandbased.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class RomiDrivetrain extends SubsystemBase {
+ private static final double kCountsPerRevolution = 1440.0;
+ private static final double kWheelDiameterInch = 2.75591; // 70 mm
+
+ // The Romi has the left and right motors set to
+ // PWM channels 0 and 1 respectively
+ private final Spark m_leftMotor = new Spark(0);
+ private final Spark m_rightMotor = new Spark(1);
+
+ // The Romi has onboard encoders that are hardcoded
+ // to use DIO pins 4/5 and 6/7 for the left and right
+ private final Encoder m_leftEncoder = new Encoder(4, 5);
+ 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);
+
+ /** Creates a new RomiDrivetrain. */
+ public RomiDrivetrain() {
+ // Use inches as unit for encoder distances
+ m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+ m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+ resetEncoders();
+ }
+
+ public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
+ m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
+ }
+
+ public void resetEncoders() {
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+ }
+
+ public double getLeftDistanceInch() {
+ return m_leftEncoder.getDistance();
+ }
+
+ public double getRightDistanceInch() {
+ return m_rightEncoder.getDistance();
+ }
+
+ @Override
+ public void periodic() {
+ // This method will be called once per scheduler run
+ }
+
+ @Override
+ public void simulationPeriodic() {
+ // This method will be called once per scheduler run during simulation
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/Main.java
new file mode 100644
index 0000000..69c9ced
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/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.templates.romitimed;
+
+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/templates/romitimed/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/Robot.java
new file mode 100644
index 0000000..75197ec
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/Robot.java
@@ -0,0 +1,102 @@
+// 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.templates.romitimed;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * 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 static final String kDefaultAuto = "Default";
+ private static final String kCustomAuto = "My Auto";
+ private String m_autoSelected;
+ private final SendableChooser<String> m_chooser = new SendableChooser<>();
+
+ private final RomiDrivetrain m_drivetrain = new RomiDrivetrain();
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
+ m_chooser.addOption("My Auto", kCustomAuto);
+ SmartDashboard.putData("Auto choices", m_chooser);
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {}
+
+ /**
+ * This autonomous (along with the chooser code above) shows how to select between different
+ * autonomous modes using the dashboard. The sendable chooser code works with the Java
+ * SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and
+ * uncomment the getString line to get the auto name from the text box below the Gyro
+ *
+ * <p>You can add additional auto modes by adding additional comparisons to the switch structure
+ * below with additional strings. If using the SendableChooser make sure to add them to the
+ * chooser code above as well.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autoSelected = m_chooser.getSelected();
+ // m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto);
+ System.out.println("Auto selected: " + m_autoSelected);
+
+ m_drivetrain.resetEncoders();
+ }
+
+ /** This function is called periodically during autonomous. */
+ @Override
+ public void autonomousPeriodic() {
+ switch (m_autoSelected) {
+ case kCustomAuto:
+ // Put custom auto code here
+ break;
+ case kDefaultAuto:
+ default:
+ // Put default auto code here
+ break;
+ }
+ }
+
+ /** This function is called once when teleop is enabled. */
+ @Override
+ public void teleopInit() {}
+
+ /** This function is called periodically during operator control. */
+ @Override
+ public void teleopPeriodic() {}
+
+ /** This function is called once when the robot is disabled. */
+ @Override
+ public void disabledInit() {}
+
+ /** This function is called periodically when disabled. */
+ @Override
+ public void disabledPeriodic() {}
+
+ /** This function is called once when test mode is enabled. */
+ @Override
+ public void testInit() {}
+
+ /** This function is called periodically during test mode. */
+ @Override
+ public void testPeriodic() {}
+}
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
new file mode 100644
index 0000000..96c81b1
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/RomiDrivetrain.java
@@ -0,0 +1,52 @@
+// 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.templates.romitimed;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+
+public class RomiDrivetrain {
+ private static final double kCountsPerRevolution = 1440.0;
+ private static final double kWheelDiameterInch = 2.75591; // 70 mm
+
+ // The Romi has the left and right motors set to
+ // PWM channels 0 and 1 respectively
+ private final Spark m_leftMotor = new Spark(0);
+ private final Spark m_rightMotor = new Spark(1);
+
+ // The Romi has onboard encoders that are hardcoded
+ // to use DIO pins 4/5 and 6/7 for the left and right
+ private final Encoder m_leftEncoder = new Encoder(4, 5);
+ 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);
+
+ /** Creates a new RomiDrivetrain. */
+ public RomiDrivetrain() {
+ // Use inches as unit for encoder distances
+ m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+ m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+ resetEncoders();
+ }
+
+ public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
+ m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
+ }
+
+ public void resetEncoders() {
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+ }
+
+ public double getLeftDistanceInch() {
+ return m_leftEncoder.getDistance();
+ }
+
+ public double getRightDistanceInch() {
+ return m_rightEncoder.getDistance();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json
index e0ebce7..9dd408f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json
@@ -8,7 +8,7 @@
"foldername": "timed",
"gradlebase": "java",
"mainclass": "Main",
- "commandversion": 1
+ "commandversion": 2
},
{
"name": "Timed Skeleton (Advanced)",
@@ -19,7 +19,7 @@
"foldername": "timedskeleton",
"gradlebase": "java",
"mainclass": "Main",
- "commandversion": 1
+ "commandversion": 2
},
{
"name": "RobotBase Skeleton (Advanced)",
@@ -30,7 +30,7 @@
"foldername": "robotbaseskeleton",
"gradlebase": "java",
"mainclass": "Main",
- "commandversion": 1
+ "commandversion": 2
},
{
"name": "Command Robot",
@@ -44,14 +44,36 @@
"commandversion": 2
},
{
- "name": "Old Command Robot",
- "description": "Old-command style (deprecated)",
+ "name": "Romi - Timed Robot",
+ "description": "Romi - Timed style",
"tags": [
- "Command"
+ "Timed", "Romi"
],
- "foldername": "oldcommandbased",
+ "foldername": "romitimed",
+ "gradlebase": "javaromi",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "Romi - Command Robot",
+ "description": "Romi - Command style",
+ "tags": [
+ "Command", "Romi"
+ ],
+ "foldername": "romicommandbased",
+ "gradlebase": "javaromi",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "Educational Robot",
+ "description": "Educational Robot - Not for competition use",
+ "tags": [
+ "Educational"
+ ],
+ "foldername": "educational",
"gradlebase": "java",
"mainclass": "Main",
- "commandversion": 1
+ "commandversion": 2
}
]
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Main.java
index 4cc64e0..9a3f9c7 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.templates.timed;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java
index 5974b41..9141414 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.templates.timed;
@@ -12,10 +9,9 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the TimedRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the build.gradle file in the
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
@@ -25,8 +21,8 @@
private final SendableChooser<String> m_chooser = new SendableChooser<>();
/**
- * This function is run when the robot is first started up and should be
- * used for any initialization code.
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
*/
@Override
public void robotInit() {
@@ -36,27 +32,24 @@
}
/**
- * This function is called every robot packet, no matter the mode. Use
- * this for items like diagnostics that you want ran during disabled,
- * autonomous, teleoperated and test.
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
*
- * <p>This runs after the mode specific periodic functions, but before
- * LiveWindow and SmartDashboard integrated updating.
+ * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
*/
@Override
- public void robotPeriodic() {
- }
+ public void robotPeriodic() {}
/**
- * This autonomous (along with the chooser code above) shows how to select
- * between different autonomous modes using the dashboard. The sendable
- * chooser code works with the Java SmartDashboard. If you prefer the
- * LabVIEW Dashboard, remove all of the chooser code and uncomment the
- * getString line to get the auto name from the text box below the Gyro
+ * This autonomous (along with the chooser code above) shows how to select between different
+ * autonomous modes using the dashboard. The sendable chooser code works with the Java
+ * SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and
+ * uncomment the getString line to get the auto name from the text box below the Gyro
*
- * <p>You can add additional auto modes by adding additional comparisons to
- * the switch structure below with additional strings. If using the
- * SendableChooser make sure to add them to the chooser code above as well.
+ * <p>You can add additional auto modes by adding additional comparisons to the switch structure
+ * below with additional strings. If using the SendableChooser make sure to add them to the
+ * chooser code above as well.
*/
@Override
public void autonomousInit() {
@@ -65,9 +58,7 @@
System.out.println("Auto selected: " + m_autoSelected);
}
- /**
- * This function is called periodically during autonomous.
- */
+ /** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {
switch (m_autoSelected) {
@@ -81,45 +72,27 @@
}
}
- /**
- * This function is called once when teleop is enabled.
- */
+ /** This function is called once when teleop is enabled. */
@Override
- public void teleopInit() {
- }
+ public void teleopInit() {}
- /**
- * This function is called periodically during operator control.
- */
+ /** This function is called periodically during operator control. */
@Override
- public void teleopPeriodic() {
- }
+ public void teleopPeriodic() {}
- /**
- * This function is called once when the robot is disabled.
- */
+ /** This function is called once when the robot is disabled. */
@Override
- public void disabledInit() {
- }
+ public void disabledInit() {}
- /**
- * This function is called periodically when disabled.
- */
+ /** This function is called periodically when disabled. */
@Override
- public void disabledPeriodic() {
- }
+ public void disabledPeriodic() {}
- /**
- * This function is called once when test mode is enabled.
- */
+ /** This function is called once when test mode is enabled. */
@Override
- public void testInit() {
- }
+ public void testInit() {}
- /**
- * This function is called periodically during test mode.
- */
+ /** This function is called periodically during test mode. */
@Override
- public void testPeriodic() {
- }
+ public void testPeriodic() {}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Main.java
index 6a895ba..ebe7ba4 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.templates.timedskeleton;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Robot.java
index b4480c8..d3968f3 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Robot.java
@@ -1,64 +1,49 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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.templates.timedskeleton;
import edu.wpi.first.wpilibj.TimedRobot;
/**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the TimedRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the build.gradle file in the
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
/**
- * This function is run when the robot is first started up and should be used
- * for any initialization code.
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
*/
@Override
- public void robotInit() {
- }
+ public void robotInit() {}
@Override
- public void robotPeriodic() {
- }
+ public void robotPeriodic() {}
@Override
- public void autonomousInit() {
- }
+ public void autonomousInit() {}
@Override
- public void autonomousPeriodic() {
- }
+ public void autonomousPeriodic() {}
@Override
- public void teleopInit() {
- }
+ public void teleopInit() {}
@Override
- public void teleopPeriodic() {
- }
+ public void teleopPeriodic() {}
@Override
- public void disabledInit() {
- }
+ public void disabledInit() {}
@Override
- public void disabledPeriodic() {
- }
+ public void disabledPeriodic() {}
@Override
- public void testInit() {
- }
+ public void testInit() {}
@Override
- public void testPeriodic() {
- }
-
+ public void testPeriodic() {}
}