From e594f42c698ddac46d25f387c17ec2952d4f6e1c Mon Sep 17 00:00:00 2001 From: Jordan Bancino Date: Tue, 2 Jan 2024 18:14:26 -0500 Subject: [PATCH] Initial import from CVS. --- .gitignore | 13 + .idea/.gitignore | 3 + .idea/SwerveIO.iml | 12 + .idea/compiler.xml | 6 + .idea/gradle.xml | 29 + .idea/jarRepositories.xml | 45 + .idea/misc.xml | 8 + .idea/modules.xml | 8 + .idea/runConfigurations.xml | 10 + .idea/uiDesigner.xml | 124 ++ .idea/vcs.xml | 6 + .vscode/settings.json | 23 + LICENSE | 628 ++++++++ README.md | 323 ++++ build.gradle | 328 ++++ gradle/wrapper/gradle-wrapper.jar | Bin 0 -> 58702 bytes gradle/wrapper/gradle-wrapper.properties | 5 + gradlew | 183 +++ gradlew.bat | 100 ++ header.txt | 16 + kit/sds-mk2/build.gradle | 24 + kit/sds-mk2/src/javadoc/overview.html | 12 + .../swerveio/module/MK2SwerveModule.java | 121 ++ kit/sds-mk3/build.gradle | 24 + kit/sds-mk3/src/javadoc/overview.html | 12 + .../swerveio/module/MK3SwerveModule.java | 214 +++ misc/virt/build.gradle | 16 + misc/virt/src/javadoc/overview.html | 13 + .../robotics/swerveio/VirtualMotor.java | 71 + .../robotics/swerveio/VirtualSwerveDrive.java | 56 + .../swerveio/encoder/VirtualEncoder.java | 62 + .../swerveio/module/VirtualSwerveModule.java | 70 + settings.gradle | 39 + src/javadoc/overview.html | 744 +++++++++ src/main/java/net/bancino/log/JsonLog.java | 118 ++ src/main/java/net/bancino/log/Log.java | 53 + src/main/java/net/bancino/log/LogEntry.java | 180 +++ src/main/java/net/bancino/log/LogHandler.java | 52 + src/main/java/net/bancino/log/LogLevel.java | 65 + src/main/java/net/bancino/log/Loggable.java | 32 + src/main/java/net/bancino/log/Logger.java | 227 +++ .../java/net/bancino/log/package-info.java | 52 + .../robotics/swerveio/SwerveDrive.java | 1410 +++++++++++++++++ .../command/PathweaverSwerveDrive.java | 324 ++++ .../command/SaveSwerveAngleOffsets.java | 72 + .../swerveio/command/SwerveDriveCommand.java | 125 ++ .../swerveio/command/SwerveDriveTeleop.java | 200 +++ .../command/SwerveDriveTeleopCommand.java | 268 ++++ .../swerveio/command/package-info.java | 32 + .../swerveio/encoder/AnalogEncoder.java | 70 + .../robotics/swerveio/encoder/Encoder.java | 59 + .../swerveio/encoder/package-info.java | 63 + .../swerveio/geometry/ChassisDimension.java | 149 ++ .../robotics/swerveio/geometry/Length.java | 200 +++ .../swerveio/geometry/ModuleVector.java | 214 +++ .../swerveio/geometry/SwerveVector.java | 302 ++++ .../swerveio/geometry/package-info.java | 35 + .../bancino/robotics/swerveio/gyro/Gyro.java | 64 + .../robotics/swerveio/gyro/WPILibGyro.java | 78 + .../robotics/swerveio/gyro/package-info.java | 48 + .../kinematics/DefaultSwerveKinematics.java | 137 ++ .../kinematics/SwerveKinematicsProvider.java | 109 ++ .../swerveio/kinematics/package-info.java | 45 + .../robotics/swerveio/log/DashboardLog.java | 109 ++ .../robotics/swerveio/log/RobotLogger.java | 77 + .../robotics/swerveio/log/package-info.java | 24 + .../swerveio/module/GenericSwerveModule.java | 546 +++++++ .../swerveio/module/SwerveModule.java | 527 ++++++ .../swerveio/module/package-info.java | 67 + .../robotics/swerveio/package-info.java | 44 + .../swerveio/pid/DefaultPIDController.java | 455 ++++++ .../robotics/swerveio/pid/PIDController.java | 393 +++++ .../robotics/swerveio/pid/package-info.java | 38 + .../geometry/ChassisDimensionTest.java | 66 + .../swerveio/geometry/LengthTest.java | 90 ++ .../swerveio/geometry/ModuleVectorTest.java | 77 + .../swerveio/geometry/SwerveVectorTest.java | 62 + vendor/ctre/build.gradle | 29 + vendor/ctre/src/javadoc/overview.html | 12 + .../swerveio/encoder/PhoenixCANCoder.java | 74 + .../swerveio/encoder/PhoenixEncoder.java | 115 ++ vendor/kauai/build.gradle | 22 + vendor/kauai/src/javadoc/overview.html | 12 + .../robotics/swerveio/gyro/NavXGyro.java | 102 ++ vendor/rev/build.gradle | 25 + vendor/rev/src/javadoc/overview.html | 12 + .../swerveio/encoder/SparkMaxEncoder.java | 106 ++ 87 files changed, 11215 insertions(+) create mode 100755 .gitignore create mode 100755 .idea/.gitignore create mode 100755 .idea/SwerveIO.iml create mode 100755 .idea/compiler.xml create mode 100755 .idea/gradle.xml create mode 100755 .idea/jarRepositories.xml create mode 100755 .idea/misc.xml create mode 100755 .idea/modules.xml create mode 100755 .idea/runConfigurations.xml create mode 100755 .idea/uiDesigner.xml create mode 100755 .idea/vcs.xml create mode 100755 .vscode/settings.json create mode 100755 LICENSE create mode 100755 README.md create mode 100755 build.gradle create mode 100755 gradle/wrapper/gradle-wrapper.jar create mode 100755 gradle/wrapper/gradle-wrapper.properties create mode 100755 gradlew create mode 100755 gradlew.bat create mode 100755 header.txt create mode 100755 kit/sds-mk2/build.gradle create mode 100755 kit/sds-mk2/src/javadoc/overview.html create mode 100755 kit/sds-mk2/src/main/java/net/bancino/robotics/swerveio/module/MK2SwerveModule.java create mode 100755 kit/sds-mk3/build.gradle create mode 100755 kit/sds-mk3/src/javadoc/overview.html create mode 100755 kit/sds-mk3/src/main/java/net/bancino/robotics/swerveio/module/MK3SwerveModule.java create mode 100755 misc/virt/build.gradle create mode 100755 misc/virt/src/javadoc/overview.html create mode 100755 misc/virt/src/main/java/net/bancino/robotics/swerveio/VirtualMotor.java create mode 100755 misc/virt/src/main/java/net/bancino/robotics/swerveio/VirtualSwerveDrive.java create mode 100755 misc/virt/src/main/java/net/bancino/robotics/swerveio/encoder/VirtualEncoder.java create mode 100755 misc/virt/src/main/java/net/bancino/robotics/swerveio/module/VirtualSwerveModule.java create mode 100755 settings.gradle create mode 100755 src/javadoc/overview.html create mode 100755 src/main/java/net/bancino/log/JsonLog.java create mode 100755 src/main/java/net/bancino/log/Log.java create mode 100755 src/main/java/net/bancino/log/LogEntry.java create mode 100755 src/main/java/net/bancino/log/LogHandler.java create mode 100755 src/main/java/net/bancino/log/LogLevel.java create mode 100755 src/main/java/net/bancino/log/Loggable.java create mode 100755 src/main/java/net/bancino/log/Logger.java create mode 100755 src/main/java/net/bancino/log/package-info.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/SwerveDrive.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/command/PathweaverSwerveDrive.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/command/SaveSwerveAngleOffsets.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/command/SwerveDriveCommand.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/command/SwerveDriveTeleop.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/command/SwerveDriveTeleopCommand.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/command/package-info.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/encoder/AnalogEncoder.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/encoder/Encoder.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/encoder/package-info.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/geometry/ChassisDimension.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/geometry/Length.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/geometry/ModuleVector.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/geometry/SwerveVector.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/geometry/package-info.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/gyro/Gyro.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/gyro/WPILibGyro.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/gyro/package-info.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/kinematics/DefaultSwerveKinematics.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/kinematics/SwerveKinematicsProvider.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/kinematics/package-info.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/log/DashboardLog.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/log/RobotLogger.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/log/package-info.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/module/GenericSwerveModule.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/module/SwerveModule.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/module/package-info.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/package-info.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/pid/DefaultPIDController.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/pid/PIDController.java create mode 100755 src/main/java/net/bancino/robotics/swerveio/pid/package-info.java create mode 100755 src/test/java/net/bancino/robotics/swerveio/geometry/ChassisDimensionTest.java create mode 100755 src/test/java/net/bancino/robotics/swerveio/geometry/LengthTest.java create mode 100755 src/test/java/net/bancino/robotics/swerveio/geometry/ModuleVectorTest.java create mode 100755 src/test/java/net/bancino/robotics/swerveio/geometry/SwerveVectorTest.java create mode 100755 vendor/ctre/build.gradle create mode 100755 vendor/ctre/src/javadoc/overview.html create mode 100755 vendor/ctre/src/main/java/net/bancino/robotics/swerveio/encoder/PhoenixCANCoder.java create mode 100755 vendor/ctre/src/main/java/net/bancino/robotics/swerveio/encoder/PhoenixEncoder.java create mode 100755 vendor/kauai/build.gradle create mode 100755 vendor/kauai/src/javadoc/overview.html create mode 100755 vendor/kauai/src/main/java/net/bancino/robotics/swerveio/gyro/NavXGyro.java create mode 100755 vendor/rev/build.gradle create mode 100755 vendor/rev/src/javadoc/overview.html create mode 100755 vendor/rev/src/main/java/net/bancino/robotics/swerveio/encoder/SparkMaxEncoder.java diff --git a/.gitignore b/.gitignore new file mode 100755 index 0000000..423a957 --- /dev/null +++ b/.gitignore @@ -0,0 +1,13 @@ +# Gradle +.gradle/ +build/ + +# VS Code +.classpath +.project +.settings/ +bin/ + +# diffutils +*.orig +*.reg diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100755 index 0000000..26d3352 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/.idea/SwerveIO.iml b/.idea/SwerveIO.iml new file mode 100755 index 0000000..07f7d4d --- /dev/null +++ b/.idea/SwerveIO.iml @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/.idea/compiler.xml b/.idea/compiler.xml new file mode 100755 index 0000000..fb7f4a8 --- /dev/null +++ b/.idea/compiler.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/.idea/gradle.xml b/.idea/gradle.xml new file mode 100755 index 0000000..ea3b65f --- /dev/null +++ b/.idea/gradle.xml @@ -0,0 +1,29 @@ + + + + + + + \ No newline at end of file diff --git a/.idea/jarRepositories.xml b/.idea/jarRepositories.xml new file mode 100755 index 0000000..f088035 --- /dev/null +++ b/.idea/jarRepositories.xml @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100755 index 0000000..06c852f --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,8 @@ + + + + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100755 index 0000000..88622e8 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/runConfigurations.xml b/.idea/runConfigurations.xml new file mode 100755 index 0000000..797acea --- /dev/null +++ b/.idea/runConfigurations.xml @@ -0,0 +1,10 @@ + + + + + + \ No newline at end of file diff --git a/.idea/uiDesigner.xml b/.idea/uiDesigner.xml new file mode 100755 index 0000000..e96534f --- /dev/null +++ b/.idea/uiDesigner.xml @@ -0,0 +1,124 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100755 index 0000000..35eb1dd --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100755 index 0000000..743973a --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,23 @@ +{ + /* Exclude temporary files from the explorer. */ + "files.exclude": { + "**/.gradle": true, + "**/bin": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true + }, + /* Always add a newline to the end of files. */ + "files.insertFinalNewline": true, + /* Aggressive formatting */ + "editor.formatOnSave": true, + "editor.formatOnPaste": true, + "editor.formatOnType": true, + /* Git settings */ + "git.autofetch": true, + "git.confirmSync": false, + /* Java settings */ + "java.refactor.renameFromFileExplorer": "autoApply", + "java.configuration.updateBuildConfiguration": "automatic" +} diff --git a/LICENSE b/LICENSE new file mode 100755 index 0000000..b133acc --- /dev/null +++ b/LICENSE @@ -0,0 +1,628 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/README.md b/README.md new file mode 100755 index 0000000..2ad1e44 --- /dev/null +++ b/README.md @@ -0,0 +1,323 @@ +SwerveIO +======== + +The Open Source Swerve Drive Library +------------------------------------ + +SwerveIO is an open-source swerve drive library written for FRC in Java. +It is pronounced "Swerve - ee - oh", which rhymes with "Oreo". + +You are currently viewing the **developer documentation**. The upstream +user documentation is available as JavaDoc documentation in the source +tree and can be easily generated using the `javadoc` Gradle task. The +change log is available below. + +Project Status +-------------- + +I am no longer on an FRC team, and my old FRC team refuses to use +SwerveIO anymore. Suffice it to say that SwerveIO's future doesn't look +too good. That being said, if you are interested in SwerveIO, please +reach out to me and let me know. I am more than happy to keep working +on SwerveIO, but right now I don't see a lot of interest, so I don't +want to waste my time on it. + +This project is as good as dead, but it doesn't have to be! Seriously, +an email telling me you want to see this thing going again is all it +would take. Until I get that email though, you likely won't see any +changes to this code. This message was written on February 17, 2022. + +Project Layout +-------------- + +This project is a standard Gradle project with multiple subprojects. It +strives to follow the Gradle-prescribed file system hierarchy as closely +as possible. + +Building +-------- + +SwerveIO can be built using a simple `./gradlew build`. Documentation +can be built with `./gradlew javadoc`. If you don't have an internet +connection or are at a competition, you can specify the `--offline` +Gradle flag. This will speed up builds dramatically because a lot of +non-essential functionality will be disabled. + +Contributing +------------ + +The contribution workflow for SwerveIO may be different from what you +might be used to. Please see my [homepage](http://bancino.net) for +instructions. + +SwerveIO is a highly specialized library designed to meet the +programming standards enforced by the style recommendations for the Java +language. The +[Google Style Guide](https://google.github.io/styleguide/javaguide.html) +is a good guide to follow, however exact standards enforced in this +library will be at the discretion of its maintainers. I personally just +use my IDE's formatting tool call it good. I do, however, have the +following rules specific to this project: + +* Always explicitly declare the type of a variable, even in lambda + expressions. In other words, don't use the `var` keyword, and if you + have a lambda that looks like this: + `(param) -> doSomethingWith(param)`, change it to this: + `(String param) -> doSomethingWith(param)`, where `String` is the + actual type of `param`. + +The goal for SwerveIO is to be the best possible library that can be +used by the most people. Contributors should write their highest quality +code. This means that SwerveIO should be: + +* **Stable:** SwerveIO should work consistently no matter what. + Whenever possible, the public-facing API should be preserved so that + users can upgrade without much difficult. Additionally, every + individual release should perform consistently every time. +* **Standardized:** All the Java sandards should be followed. This is + a Java library, it should look and work like one. +* **Efficient:** Code should be clean and efficient. +* **Versatile:** Remember that the design goals of SwerveIO intend to + cater to the most swerve drive users as possible. Code should not be + specialized or dependent on any hardware or 3rd-partly library + behavior when possible. Great care has been taken to ensure the + library is abstract enough that new hardware can be easily added. +* **Documented:** Always keep the documentation up to date and write + comments wherever they are needed. Currently, there are more lines + of comment in SwerveIO than there are lines of code. I am proud of + that. + +Before contributing to the development of SwerveIO, please be sure of +the following: + +* You accept the terms of the license that your contributions will be + released under. See the `LICENSE` file. +* You agree to the contributor code of conduct, detailed below. +* You took a look at the `TODO` file. That file contains what's + planned for the future of SwerveIO, so maybe you can help out with + some of the items listed there. + +When making your changes, be sure to add an appropriate entry to the +changelog. + +Environment Setup +----------------- + +You need the following pieces of software to develop and contribute to +SwerveIO: + +* A Java Development Kit (JDK), version 11 or greater +* CVS +* An IDE or text editor + +I don't have any hard requirements for the IDE or text editor you use, +but I would highly recommend using VSCode or IntelliJ because I have +both configurations bundled to set up the project automatically for you. + +Code of Conduct +--------------- + +SwerveIO's development code of conduct is simple: + +* Be human. We're all people here and we may all be different and at + different stages of life. Be understanding of others and know that + they may be going through something you aren't aware of. +* Don't get political. There is a place for politics, and if you know + me personally, you know I am quite opinionated. But SwerveIO's + development cycle is not the place for politics. The focus should + just be on writing code with fellow programmers and helping to + create the best library possible. + +Release Checklist +----------------- + +This is just to help me when I'm releasing new versions of SwerveIO. + +* Increment version in `build.gradle` +* Add changelog entry to this readme. +* Tag the release with the version number and publish it to my + website. + +Release Model +------------- + +SwerveIO may be under heavy development during robotics seasons. Because +of this, it featured an aggressive release model in its early days that +wasn't afraid to break existing code. However, as I want SwerveIO to be +stable, and I have grown more conservative over the years, I want to +slow things down and keep things stable. I am open to the idea of +maintaining long term support (LTS) releases along side the primary +development cycle. I want SwerveIO to work for its users, so please +contact me so we can discuss SwerveIO's release model. + +SwerveIO strives to adhere to [Semantic Versioning](https://semver.org): + +> Given a version number MAJOR.MINOR.PATCH, increment the: +> +> 1. MAJOR version when you make incompatible API changes, +> 2. MINOR version when you add functionality in a backwards compatible +> manner, and +> 3. PATCH version when you make backwards compatible bug fixes. + +Change Log +---------- + +8.0.2 +----- + +- Updated the contributing instructions to note the usage of CVS + instead of Git. +- Added a notice to this README announcing the deprecation of SwerveIO. + I'm sad to see this project go, and maybe I'll revive it at some + point later in life, but if nobody expresses any interest in what + I've done here, then I'll lay this project to rest. I'll of course + still host all the source code here, and if you're considering forking + SwerveIO, I'd say reach out to me and email me because I want to be + involved! + +**Note:** This release makes no code or build changes. It simply bumps +the version number so that I could update this README. For that reason, +I have no idea if SwerveIO still builds with current tooling, but I know +that if for some reason it doesn't, we can get it going again in no +time, just send me an email. + +8.0.1 +----- + +- Updated CTRE Phoenix to the latest kickoff release. This fixes a build + error that some Windows users were encountering when trying to build + against the local Maven repository. + +8.0.0 +----- + +Welcome to the 2022 Kickoff! This release includes all the changes +from `8.0.0-rc1` and `8.0.0-rc2`, so if you're upgrading from SwerveIO +7x, make sure to read the release notes for those too! + +- Updated WPILib to '2022.1.1', the kickoff release of WPILib. +- Upgraded the SparkMax API to REVLib 2022. + - The `SparkMaxEncoder` class constructors were updated to reflect + the changes made to how REV does encoders. Some breaking changes + were made for users that weren't using the default constructor. If + you were using the default constructor, you should notice no + changes. +- Removed the `LegacySpeedController` class introduced in `-rc1` because + all vendors now support WPILib's `SpeedController`. + +8.0.0-rc2 +--------- + +- Updated WPILib to `2022.1.1-rc-1`. +- Updated OpenCV to `4.5.2-1`. +- Made some internal variables `final`. + +SwerveIO 8.0.0 will be released when WPILib 2022.1.1 is, which will +most likely occur before or during the 2022 kickoff. At this time, I +am considering breaking all WPILib support off into a separate Gradle +subproject, because of the recent breaking changes. SwerveIO will still +fully support WPILib, but will also provide all its own interfaces so +that it can be used entirely separate if necessary. + +8.0.0-rc1 +--------- + +SwerveIO 8x brings SwerveIO up to date with WPILib 2022.x. This release +should look a lot like the previous releases. However, WPILib made a lot +of breaking user-facing API changes. This update is simply to reflect +those changes and make SwerveIO compatible with future releases of +WPILib and vendor libraries. + +**Note:** This release of SwerveIO is not compatible with WPILib 2021 or +earlier. It *requires* WPILib 2022.1.1 or later. + +The breaking changes are as follows: + +* Replaced all references to the deprecated `SpeedController` + with the new `MotorController`. All interfaces and implementations + were adjusted accordingly. + +Other non-breaking changes are also included in this release: + +* Replaced a reference to the deprecated `DriverStation.getInstance()` + with just `DriverStation`. +* Added the `LegacySpeedController` class to support vendor APIs that + haven't updated their APIs to use the latest WPILib. Note that this + class is already deprecated and schedule for removal as soon as all + supported hardware vendors update their APIs or WPILib removes the + `SpeedController` interface, whichever happens first. +* Cleaned up this README document to reflect the current state of + SwerveIO. +* Updated the copyright headers throughout the code. + +7.0.4 +----- + +* Updated the IntelliJ configuration to use the system's JDK instead + of a hard-coded JDK. +* Updated Gradle from `7.2` to `7.3.3`. +* Updated WPILib to `2022.1.1-beta-4`. +* `[vendor/kauai]` Updated NavX library to `4.0.435`. +* As far as I can tell, it looks like CTRE Phoenix is working again. + As of the date of this release, SwerveIO can be built normally. + +7.0.3 +----- + +* Added an IntelliJ `.idea` folder for developing SwerveIO with + IntelliJ. Since I'm no longer on the robotics team, I don't use VS + Code much anymore, so I've become much more familiar with IntelliJ. + Obviously IntelliJ cannot be used by students, because students + require the WPILib VS Code plugins, but since I'm not going to be + directly deploying SwerveIO to a robot, I can use IntelliJ. +* Updated Gradle from 7.1.1 to 7.2. +* Updated CTRE Phoenix from 5.19.4 to 5.20.0. Note that at this time, + neither version will build with SwerveIO because the CTRE Maven + repository is broken. It does not match what is in the vendor JSON + description file, and indexing is not allowed, so I'm unable to + troubleshoot. If anyone knows what's up with this, or has a fix, + please contact me at + [jordan@bancino.net](mailto:jordan@bancino.net). +* Documented the LogIO classes. + +7.0.2 +----- + +Updated Gradle from 7.0.1 to 7.1.1, and fixed some broken links. + +7.0.1 +----- + +Fixed some errors in the Javadoc home page that prevented the +documentation from building correctly on JDK 11. + +7.0.0 +----- + +SwerveIO 7.0.0 cleans up a lot of the API, making for neater and less +fragmented code. This release also simplifies the build system and +documentation, and attempts to decouple both from Jordan Bancino's +infrastructure so that SwerveIO is fully self-contained. + +* Removed all methods and classes that were deprecated as of the + previous release. +* Removed all kit module constructors that referenced angle offsets, + because angle offsets are now handled at the `SwerveDrive` level + using `saveAngleOffsets()` and `loadAngleOffets()` +* Merge LogIO directly into the SwerveIO source tree. LogIO is now a + part of SwerveIO Core, instead of being it's own artifact. The + package name will stay the same and this change will be reversed if + LogIO becomes needed for future projects. +* Update dependencies: + * **Gradle:** 7.0 -> 7.0.1 + +6.1.2 +----- + +This release doesn't make any changes directly to SwerveIO; it is simply +a dependency update: + +* **WPILib:** 2021.2.2 -> 2021.3.1 +* **Gradle:** 6.8.3 -> 7.0 + * JCenter is being shut down, so now certain dependencies are + loaded from Maven Central. diff --git a/build.gradle b/build.gradle new file mode 100755 index 0000000..cb2f963 --- /dev/null +++ b/build.gradle @@ -0,0 +1,328 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +/* + * SwerveIO core build script. This is the master build script that holds the common + * project configuration for all SwerveIO subprojects, as well as the main SwerveIO + * core project. + */ +import org.apache.tools.ant.taskdefs.condition.Os + +def versions = [ + 'SwerveIO': '8.0.2', + 'WPILib': '2022.1.1', + 'OpenCV': '4.5.2-1', + 'Jackson': '2.10.0' +] + +/* Not all "projects" are actual projects. This filters out the ones that aren't. */ +def matchProject = { Project proj -> + return file("${proj.getProjectDir()}/build.gradle").exists() +} + +/* + * Check if there is an internet connection. This is used to dynamically + * disable build functionality that requires a connection, because those + * functions will fail when we are at a competition. + * + * This function checks if the --offline was specified before attempting to + * establish a connection. + */ +def isOnline = { + /* If we know we're in offline mode, we already know the answer. */ + if (project.getGradle().startParameter.isOffline()) { + return false + } else { + /* There may or may not be an available internet connection. */ + try { + final URL url = new URL("http://wpilib.org") + final URLConnection conn = url.openConnection() + conn.connect() + conn.getInputStream().close() + return true + } catch (MalformedURLException e) { + throw new RuntimeException(e) + } catch (IOException e) { + return false + } + } +} + +/* Add external javadoc links if an internet connection is available. */ +def javadocAddLinks = { Project proj, String... urls -> + if (isOnline()) { + configure (proj) { + javadoc { + for (String url : urls) { + options.getLinks().add(url) + } + } + } + } +} + +/* Execute a command on the operating system. */ +def osExec = { String command, List arguments -> + new ByteArrayOutputStream().withStream { OutputStream os -> + exec { + executable = command + args = arguments + standardOutput = os + } + return os.toString() + } +} + +def lsRecursive +lsRecursive = {File dir, String matches -> + def list = [] + dir.listFiles().sort().each { File file -> + if (file.isFile()) { + if (file.getName().matches(matches)) { + list += file + } + } else { + list += lsRecursive(file, matches) + } + } + + return list +} + +/* Export methods and variables to subprojects. */ +rootProject.ext.versions = versions + +rootProject.ext.isOnline = isOnline +rootProject.ext.javadocAddLinks = javadocAddLinks + +/* Root project tasks */ +task updateHeaders() { + doLast { + def header = file("${projectDir}/header.txt") + def patch = file("${projectDir}/header.patch") + + patch.text = osExec('git', ['diff', "$header"]) + + if (!patch.text.isEmpty()) { + def patchFiles = lsRecursive(file("${projectDir}"), '^.*\\.(gradle|java)$') + for (File file : patchFiles) { + println "Patching '$file'..." + osExec('patch', ["$file", "$patch"]) + } + def deleteFiles = lsRecursive(file("${projectDir}"), '^.*\\.orig$') + for (File file : deleteFiles) { + file.delete() + } + osExec('git', ['commit', "$header", '-m', 'Update header.']) + } else { + println "Headers up to date." + } + patch.delete() + } +} + +/* Common configuration for all projects. */ +configure(allprojects.findAll { matchProject(it) }) { + /* Java plugins */ + apply plugin: 'java-library' + apply plugin: 'maven-publish' + + /* Maven metadata */ + group = 'net.bancino.robotics' + version = versions.SwerveIO + + /* Java 11 */ + sourceCompatibility = 11 + targetCompatibility = 11 + + /* + * SwerveIO doesn't need any of this stuff without internet. This + * will dramatically speed up the time it takes to build SwerveIO, + * which can be helpful at competitions when things need to happen + * quickly. + */ + if (isOnline()) { + /* Maven jars - for documentation and IDE support. */ + java { + withJavadocJar() + withSourcesJar() + } + + /* All Javadocs should link to the standard Javadoc, and the WPILib Javadoc */ + javadocAddLinks(project, + 'https://docs.oracle.com/en/java/javase/11/docs/api/', + 'https://first.wpi.edu/wpilib/allwpilib/docs/release/java/' + ) + } + + /* All artifacts go to the root repo. */ + def repoUrl = "${rootProject.buildDir}/repo" + + /* Publish to a local repo that can be published separately. */ + publishing { + publications { + /* Artifact is derived from the project name. */ + mavenJava(MavenPublication) { + artifactId = project.getName() + from project.components.java + } + } + + repositories { + maven { + url = repoUrl + } + } + } + + /* Common Javadoc configuration. */ + javadoc { + options.overview = "${projectDir}/src/javadoc/overview.html" + options.setWindowTitle("${project.getName()} ${project.version} User Documentation") + /* All javadocs go to the root directory. */ + options.destinationDirectory(file("${rootProject.docsDir}/${project.getName()}")) + } + + /* + * Dependency repositories. All repositories must be shared across all projects so that they can include + * each other without duplicating the repository requirements. + */ + repositories { + /* Jackson XML & Vendor: KauaiLabs */ + mavenCentral() + + /* WPILib */ + maven { + url 'https://frcmaven.wpi.edu/artifactory/release' + } + + /* Vendor: CTRE */ + maven { + url 'https://maven.ctr-electronics.com/release' + } + + /* Vendor: RevRobotics */ + maven { + url 'https://maven.revrobotics.com/' + } + } + + /* Generate GradleRIO vendor Json files that can be dropped into vendordeps/ */ + task generateVendorJson() { + def jsonFile = "${project.getName()}.json" + def repoList = { + def str = "" + repositories.each { + str += "\"${it.url}\", " + } + return str.substring(0, str.length() - 2) + } + def outputDir = file(repoUrl) + doLast { + outputDir.mkdirs() + file("${outputDir}/${jsonFile}").text = +"""{ + "cppDependencies": [], + "fileName": "${jsonFile}", + "javaDependencies": [ + { + "artifactId": "${project.getName()}", + "groupId": "${project.group}", + "version": "${version}" + } + ], + "jniDependencies": [], + "jsonUrl": "", + "mavenUrls": [ + ${repoList()} + ], + "name": "${project.getName()}", + "uuid": "${project.group}:${project.getName()}", + "version": "${version}" +} +""" + } + } + + /* When publishing Maven artifacts, also generate vendor JSON. */ + publish.dependsOn generateVendorJson + + /* All common dependencies go here. */ + dependencies { + /* WPILib */ + api "edu.wpi.first.wpilibj:wpilibj-java:${versions.WPILib}" + api "edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:${versions.WPILib}" + api "edu.wpi.first.wpimath:wpimath-java:${versions.WPILib}" + + /* + * The SwerveIO core itself doesn't use these artifacts, but the vendor dependencies, + * included in the subprojects may, so we include them here to ensure that they are the + * same version across the board. I noticed, for example, that the NavX library would + * pull older versions of these artifacts than what SwerveIO was using, so we do this to + * make everything line up. + */ + + api "edu.wpi.first.cscore:cscore-java:${versions.WPILib}" + api "edu.wpi.first.cameraserver:cameraserver-java:${versions.WPILib}" + api "edu.wpi.first.ntcore:ntcore-java:${versions.WPILib}" + api "edu.wpi.first.wpiutil:wpiutil-java:${versions.WPILib}" + api "edu.wpi.first.hal:hal-java:${versions.WPILib}" + + /* Required by WPILib - Make sure this lines up with the version specified by WPILib */ + api "edu.wpi.first.thirdparty.frc2022.opencv:opencv-java:${versions.OpenCV}" + + /* Required by WPILib - Gradle emits warnings if this isn't included. */ + implementation "com.fasterxml.jackson.core:jackson-annotations:${versions.Jackson}" + + /* Unit testing */ + testImplementation 'org.junit.jupiter:junit-jupiter-api:+' + testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:+' + + /* Convert OS into WPILib platform */ + def platform + if (Os.isFamily(Os.FAMILY_WINDOWS)) { + platform = 'windowsx86-64' + } else if (Os.isFamily(Os.FAMILY_MAC)) { + platform = 'osxx86-64' + } else if (Os.isFamily(Os.FAMILY_UNIX)) { + platform = 'linuxx86-64' + } else { + throw new GradleException("Unrecognized operating system.") + } + + /* JNI stuff so WPILib can run on the desktop. */ + testImplementation "edu.wpi.first.thirdparty.frc2022.opencv:opencv-jni:${versions.OpenCV}:${platform}@jar" + testImplementation "edu.wpi.first.hal:hal-jni:${versions.WPILib}:${platform}@jar" + testImplementation "edu.wpi.first.wpiutil:wpiutil-jni:${versions.WPILib}:${platform}@jar" + testImplementation "edu.wpi.first.ntcore:ntcore-jni:${versions.WPILib}:${platform}@jar" + testImplementation "edu.wpi.first.cscore:cscore-jni:${versions.WPILib}:${platform}@jar" + } + + test { + useJUnitPlatform() + testLogging { + showStandardStreams = true + } + } +} + +/* Common configuration for subprojects only. */ +configure(subprojects.findAll { matchProject(it) }) { + /* All subprojects depend on the core SwerveIO project */ + dependencies { + api rootProject + } +} diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar new file mode 100755 index 0000000000000000000000000000000000000000..cc4fdc293d0e50b0ad9b65c16e7ddd1db2f6025b GIT binary patch literal 58702 zcma&OV~}W3vL#%;<*Hk@ZQHhO+qTVHwr$(CZQFL$+?np4n10i5zVAmKMC6WrGGd+F zD|4@NHj-D$z)bJV;MYNJ&!D%)v-fQ%q0JG$_z5GVUJTPg0MHPf1TvicY#6DXYBBQ4M`$iC~gA;06+%@0HFQPLj-JXogAJ1j+fRqw^4M` zcW^RxAfl%+w9SiS>QwBUTAfuFAjPXc2DHf6*sr+V+jLQj^m@DQgHTPmAb@F z8%GyCfcQkhWWlT31%4$PtV4tV*LI?J#C4orYI~WU(cSR{aEs^ycxY`1>j1po>yDMi zh4W$pMaecV*mCsOsPLxQ#Xc!RXhpXy*p3S2Hl8t}H7x#p5V6G5va4jV;5^S^+>+x&#zzv4!R}wB;)TyU zE_N~}nN>DTG+uZns%_eI=DL1E#<--Sccx30gvMT}^eu`2-u|{qQZ58(rA2aBYE*ZD zm|*12zg*@J$n|tbH%Mp|d|O9W%VT~xG})R=Ld5z<(z%DOO6=MF3Xh-aF%9Hf$?1N9%8Pkev{wun$jZ2 z^i*EhRt8Ve<7`Wyz~iMZDye+XVn}O%qbhV`wHL+%P+n)K&-UMuZw^RRfeQ)%K=k*m zq5l7mf`4K_WkV5B73~MxajljrjGiJqpiV#>0FkyyrB)@HY!;Ln(7JJ*W(>d5#^ubU zVAkTMs*CHzzvUa^nRu0*f-(ek+VZw+@P~}a;;(K=|!9Mhv(~y-mlW);J zb&bB=vySHG`u?j&_6dh^*se*l_B3avjlE|!!Cb0pXyEXRbLy*@WEQ4|)M<`p8Q!rfDJ2RI!u1hPzNjy&)(kcY~GaD6?)7#dCbm`NFh?Y_g$#!+Qrie7%<7P}<-+W@{sxi4JYI{iY zk0(>m$DxOI=~-&eXf2bfh^&(U@o)>(iA1_wJ%B(+nFH+ceib%HEck32QL=J(BNFh`f>St1%llF8chX7#cp*;z}& zcTeXkwsXhf+e;##!FS2yi=2cChcYfzm$wQJ z9%4kAq)wLHf5wfcj!A|xDsAiAOHRzf*)Z-|daN9y5jK-*R{Q0?xaSX-3m|WeuZ`BJ z>eTi@uQ{OGSDIJ#Iu@JPtOy!C?q)g*6SHORg)eAJGh8b-I*X_+xNqZ|OXEsQ-RWte ze`zjjeV9PpE3ac2za+Rs=PA;%QZ>T{x(TRzwWLp_X^2yC-DOEMUy5So!npzL&-@}u z#>uK#&`i&c%J$!bsntEJhY@rF(>6eY;6RoI5Qkn!&<80X5+1(x$T|wR-ad?4N1N^a0)nBj#&EkVvQ?I_+8t*%l#VK&I?uo$ERI1HMu4P2rLMeH%m3 zZ|HA^*O^dA$gb$`Cw;z9?G?m3@nH6TNYJ04Fd-M2wp8@(;vAvJ ztFoni)BLwncQ3@cO*^+6u;(&D<;N;RKb)_NQ_Qu&?@h3MWvo>6FHG%%*smTwj3;dG zQJnT7Wb?4!XmV^>N@ZkA7Jv9kAfD-gCHu2i+!A!}y98SO><8g}t;1JOOxj>#l zM!?y|j5fR3WY2(&_HSGjgMa?Zif<M@d8W z)4>Ptm@zj|xX=bbt$=j}@a_s|xdp6-tRlq6D|xb_;`9oJlkYF1AH%?Pzv$eIAogMi zf(_H*5t({Arfs5XAPj46pjiudQw?dulW-=OUqBVa)OW9E;^R+NDr&LES&m_nmP>Ga zPf)7_&Gn(3v1qu_a^qW9w4#XIEfgiHOQ(LDi=E&(-DcUSfuQE0`ULsRvS}fpS@<)3 z|CbQSi49rU{<4|XU;kiV|C7}Gld$}Yh5YXjg^W$~ovobybuZ^&YwBR^=qP3G=wxhT z?C_5Trbu~95mOoIXUmEOY646_j4ZL)ubCM{qFkl1u*%xs%#18a4!(*b<&edy<8t2w z_zUxWS5fypUp9ue+eswoJSyv*J&=*3;2;q9U?j>n^q?)}c8+}4Ns8oToBJgD;Ug=y zOa0>{VFrLJutjR{PJmm(P9lPzoPi{K!I{l)pGwDy59p-uxHB9I&7zl11lkCu(}*A< zh492AmxsgwEondBpB^{`I*L&Ut40fjM^JS8VdAWQMlwc>_RUM5|Mjes!36DGqW`xs z4tU4`CpOk|vew8!(L}fEvv5&-3#GqZ(#1EZF4ekDQ@y*$tMDEeG?nOUiS-KXG=rAZ zHUDlMo@X&yzo1TdE6b6!s#f{*45V-T3`e2)w5Ra3l>JWf46`v?Y6B&7*1$eS4M(3% z9C~G@N@RXm)8~EXL*9IObA+PwD)`%64fON_8}&pqjrg|2LmP{W^<0@W`9s^*i#F}V;E8~`-}(4@R4kz?t(RjA;y-r%s^=)15%C> zbF;NZET~nybEsmUr8sH^Hgq^xc^n$ZP=GcZ!-X-Go7J4nByj8%?aQ`c{88;p15Kf>|0h+5BLkM&@KI-(flp^npO3MC~W@Uyjv* z6Hu!4#(NtZJ0*;_{8^xcLrC4-zK$BVo7S5V=eg?R8P;BOpK3Xwms+Jt-8R6us zf_rUHFYHn~lu!)U$e$#%UBz7d8YS;mq}xx$T1PIi=4={c-_cY6OVc<=){mOVn>~J$ zW*2PB%*40eE^c+d=PP7J@bqIX_h4u6b6#W|ir<;IlR`#s`Q*_Z8Q?*s_&emuu8D;NSiPX9mK?>$CwcbjhCuv zO&u(0)@}8nZe=Fl*0uMri02oYDjs#g$OHCZ6oTXV2Y0TrZ}+o%{%i)OAJBj2xHC|F5o+`Qmq`$`2EaL=uePwq%k<;6S2n=w%_9vj$8NO|{` zTEg*tK8PU#DnQ#dQ2mMJaaL|HV;BCn?eQ%d0vY@S7Pu@7 zsf5u`T=bL7NfyYO?K^PR_|jap@K|qQ zmO8CK+&O3fzgEnp2|_=^K9ln~QhxjgMM>EQqY@k@@#np@FnZq|C{EyEP7^NurUm0q zW5rKmiy%__KE>YItATyMhE({0%ve10la=mUd<^AcB{T_$Y`2_N-x;F#3xTORXvhPZ7psmqhXy?WxxB5w!m*4&Q;?t$4Kt?m_em-htVDxora24&6~5z$MG(RT{trtp(L( zy&VDT{@p9_DGoq+I|abw$E!TyTO7j6dWQ25dqdKV*z3E?n-p|IG42ZUnNok? zY4K{y{27bUT@#|Zcni!tIgjE`j=-0rl(tVlWEn>5x7BJBkt0iw6j^4n1f2i^6ebo; zt^&Yb##}W0$3xhH&Nz*nANYpO$emARR6-FWX;C?(l7+}<97Ay#!y%BI6^st=LaJ>n zu{ORVJ9%`f*oy85MUf@Fek@T_+ML0-0b$lkEE2y8h%#P^X6+cn)IEXa@T7CQ{fV z-{^wJGN*+T!NsAH@VNM3tWG;%y{pVF2m z2*0+i?o40zSKVq_S18#=0RrJIse+;5cv#a`*`wNs+B%Ln8#e0v^I>7a_33h?lHo14 zg)CbDfGMyH2cj%7C`>|Rrg;U?$&y!z(U10>(dHKQsf9*=z)&@9u@w%y+e@*CnUS|E z*O^cQqM*!sD|e!u(yhXPi$Sl<$daf3sq@Iexafxt3F#2R&=cK z!gT-qto{oVdGUIxC0q`tg)B-Zy(pxGx}&svoA}7p=}jb3jEjQ!v6=afKI!2`&M{#tY$~3LR}#G#U2up2L{} zMGSX>Yjg6-^vWgeX0i;Nb0=gQmYa!|r0rRUshm2+z3AlehjfTqRGnRAmGhHY3`R_@ zPh4GAF@=nkRz;xMO3TPh$)9Iq?Fs5B@~)QIntSyeBy^10!ts?9Z@tK&L6xJd9 zNzaaz6zvrtr&MPQ@UD)njFUtFupwB zv+8%r`c@#asm}cKW^*x0%v_k3faHOnRLt7vzVFlqslue32rt(NNXnkS+fMSM&^u)8 zC`p{on>0pf=1id|vzdTnBLB;v%*ta`o_lzj21u+U-cTRXR%sxE%4k<(bU!orfsJ&v z3FLM2UT_*)BJm1^W;Z{0;z^_e=N&QXSO>rdB`*cp>yGnjHJt$ zcJd~52X&k1b<-`2R{bqLm*E(W{=|-)RTB*i$h4TdV12@beTkR&*iJ==ck*QlFiQ52 zBZ|o_LP06C?Sgs3VJ=oZQU0vK6#}f9gHSs)JB7TU2h~}UVe%unJA!URBgJ# zI~26)lGD4yk~ngKRg;(s4f@PccDZaL{Y=%6UKHl&k|M@Zc4vdx-DX4{belQ);URF? zyxW+|Ziv}%Y!sFdY@YO))Z|f34L(WjN*v#EfZHn6m)X@;TzQ@wIjl4B_TieZY}qY`mG}3VL{w?; z&O>sZ8)YnW+eLuW@rhClOOCZe2YP@4YWKN?P{c~zFUj*U?OayavPUo!r{uqA1<8h! zs0=rKKlwJYk~34F9$q6fQ&jnw_|@cTn{_kA8sUZ#2(Lb@R$NL*u>08yYGx{p6OeX~ zr7!lwGqMSury(v5=1_9%#*MORl2apGf(MQIQTMN35yE3l`^OS7r;SKS6&v-5q}Gw* zNWI*4OKBD&2YbCr8c{ifn~-9w-v+mV49W+k)$jjU@WA+Aok01SA#X$Sspj}*r52!- zNqOS<0%uMUZeSp+*i1TEO$KGKn7EwzW=s?(b5X^@3s5k*80ns2I2|bTHU+bWZ$x;j z`k@>)1G#JgT=F!8awgol?DqK^S4R*g?e}2rOYRVMUKKxSudO(hOLnnL zQqpxPNouLiQFYJs3?7!9f6!-#Pi83{q3-GgOA|{btKup4fYDu-JFOK~Q1c3KD@fdJ z?uABYOkHA^Fc~l0gTAy4geF<-1UqdS=b=UM6Xi30mPhy1-f^aQh9H(jwFl5w*X`Mh z=Ee5C?038GEqSVTd!67bn9*zQg-r8RIH3$$ zf8vWEBbOc`_0U{b)t)Toa~~<7c-K_=G%*iTW^?6mj9{#)@|# zku9R^IDzbzzERz~fpxFrU*it;-Iu&m!CAtM&$)6^2rMyV4 z$+e!$(e)!UY(Sc9n6hkr^n&cvqy8}NfZz+AQc8fU9lNczlP>5D3qzWoR55YvH94^* z-S%SVQ96pK3|Yo`75D&85)xij9Dl8AO8{J*{_yhs-KtsLXUYqwieO(nfrkB@%|OyI>yF+1G?m7>X&djb(HBNNw3KX;Ma*oMV)cV0xzxmIy+5>yz>l_LLH)VyRnYYce zw$?q!hJzX0TlE0+o5QJDM~sPrjVCN7#|32#rUkc>?-eN6Q0RqQTAl~`&isrQg)ass z+x5XapaYh{Dj`+V096?w)w2!Cnmh?x1WmFC$jEFY4;V)XAl3*tBS)V)3TbL)g46_g zCw9pl^!3OCTOcaEP!?==guEAw;VZ}fE6K-;@qD-Rx~td+j(N>)Wv$_mqFTH_wVZNEEuDG!0T`HXLsf+_E=X3lw4`_&d5&YMl%H733ckO){vZm znFLS`;5J#^`5~unet`V#*Y5In3yb|Ax z|A6b^F37!_z$_{6h{7l~<{u7{Fx*A*#zw{GD)6e}n6f<|)&7`S-txiz3Jm4S5hV&8 zm|Ncc{j_~`^pQ*I#w21;(jwi8GnH4efO;R|r4$tH~i;Bcmp^sP9) zjhJne@yzU&XvFNoc~i(wQ?nE`o6Hk~!;x(%xh7?zvigH2g`!v8L-vEN0DvV3?m( zSW(TZ%2AWf`rS}GGMqUj!8yCp#|fR--Vxfj=9}YD97Gocdj=S z0zkF-jsO>EcPTB1zRO$++k^bH%O`=UkHdHT^5?{$)ot<-K2XIE7js*4OjF)BsVjCJ z*KN)!FdM*sh=fB$p8*EzZmGJp?B_=a-90$FI{S$LLjBU$(lxUj;9 zIBszmA*129W+YE;Yy{J~3uyOr<2A(`*cu0IJN#tmUfz2jIWQi_h)_-V6o+5CjbX!1$lz6?QYU za&|O#F%~hmGUhil{M+J|*0<3&{a1%ONp-^!Qx*LOTYY}L!r9BbTxCjHMuUR0E(uH` z!b$*ZMdnB{b2vsb<&P6})+%O=%a8@~$fjbtfF@Z>^Q@enTOJ%VT)Rdc!wX|@iq9i}HaFZAeY6g8xGZY7h-r1sy_<#YU6}I?L zwvf0ePE5PKbK>2RiJOFO5xNhMY+kt`Qi?Oxo&@xH$<^Q;Nb(&rjPBAcv;XtmSY90z z;oIFFl%lDq$o&kYQ;aSHZHD@W({Y1hw<-I>7f_X8wc?%hNDlo~Ig;63RlHNhw~#R3 zA*f5D_Qo`4_ajY4Gr{mLs*(Fxh(U%oua_u3r%`H!TI)@R!!iqV8IOhIOzI@=7QJ=G zV$(9mEVL(7DvPn0j%_cOZN|vvNg8*PHma`6+oS;PDz%iOFyo0n0e%$<#A3r~$=I0T zDL*{AREUGx&C2}?I9cVL`UcPyawTqA4j-4%Mr-4`9#8GX1jiJkKGpHVr1~Rj#zFaZ zqmE!<|1JCi!LDG?1^Ys62xz(p;Uu!QZB7!C0#piy1_9=e?^s@-sd1gs!h$;Q`TNtf z3N4Elsgl#={#U`~&}FNvH78MLjjavl1x*4pNVr338>%sfHu>bxo2#eZN2ee9q#*Jg zDk_=OBR;8t6=pBN0aj)&Nj}pzqqUYW(tfk?bXTdKbNQFSUMCyN-!b0#3?Z;ijzx$M z^Eo6Eq*NO!Y8K;84H4MHj_xwBYc|3>+D(PFj7ejhECG@5@Pk&8dG<)HwwO2~j7KV6 z0$s}=*D;ek#8$a*sxVlC_`qFkM0%BQQ@v2H&Aq@G9XCQt^^x<8w*=MbZV)@aPrrn; z`6r*&f`x&1lp)`5>-|-4%l&W4jy~LydfN;iq?Y8Xx>Sh#2Lx@FXo|5{WKp@y-x;)7 zl;;_Y*-Nu3pcH-)p0(tP~3xO_u~>HpCdEfgyq7V-!ZZ{?`6v_b-vx< zuu|gm5mG6c@D{FYMLuzvG+A2T&6&`n>XM%s`+Qtj)5XdpyFOnz3KLSCOxaCEUl()M z3b~FYqA3FT1#SY{p36h%M^gBQpB2QzEdtM9hMBMRMu{|rf}(;S85&|A!|Aj}?fMKaju!y>_AS}#hRe_!&%8V=6+oPPtE zOOJ-Rcrf>hNq@lG{{@$H?6ikt@!A2OePLe{MBIWSPz7{u(I} z$PXzD;leHG?Xl0FnWt+Wrkrk*|e3P~YVF@N$y&L929cc=#-!*k)HZKDo8!#+t|?9p0z1KSDKclB&M6~hN5<9~^DIltXKR$+iK*h9k$|@Qoy9H}PSI;b(v>w`8(k70@sfa4nRweeiwZ-syP3zPSsyK_8Te9*(FQdm+ z84ZDah4PGehH72w=Q8bx;pK5juT67rJKb|ovD#COI^l6z0eBidn$!Y?T2;5sN+vTV z$`%Edb<%-Oq@NPZy<2Z3m;$}!9JzIuVK6;fJi>>m3q!Lr!2xXRq+l0LvZIR_PNYrP57E#sCvD^4UU2GVr*Rx`QcT}yQanF z3i~!-2Vkk4S%4Hd2baDvrM2g(&1jZaA1!vLi!I#5wX6g^&PE`0-TovM(%wuaPXAno z`a&j{ai=TsgKpc1C3|)tY#!4>SPBbMnchi}glCBwaNE(4`gi}JY0;`|m`s{HtaP@& zHxwCt#2&z9A7O+=v>za}LW~}G>_tWo$dsRX)f1L=+tZF5E&RBA#jUC|N9ZPa_&z5= zekCOsIfOh`p(&S8dnkE~9#(;BAh8qzi5JYT0nP7x&Hga3v`XFdRN|$5Ry#mq*AN$J zV)l~LSq}2d{EJ@%{TLnkRVn*sdM{_b|4!x73|Ux9{%S;FPyhfZ{xg;P2ZmMuA*cMG zipYNeI7{u98`22!_phwRk|lyX#49r%Lq1aZAabxs6MP79J3Kxh0z1E>MzLS6Ee5u+ z@od~O#6yMa;R}eI*a|ZB$ar0BT`%X4+kyxqW4s+D3rV176EAsfS**6-swZ9OIPRZ& zlmIH>ppe;l28`Kd0z(alw^r<%RlDpI6hv)6Gs?GIpffKApgx^)2-6jAzjZE0BtPBC z0z8!#C5AP${zTF$-Z^v%^ie8LI*rvR+*xc=>fa;`SRUSLAio?qL;jVFV1Bw4K>D+i zyEQ}vyG2HTx>W?Ul&MhxUXK7n;yfN)QS`foM!4>4-(PGwxW!^^UyKOz(v+1BejI*& zQSkV|m5=JF4T0k*+|h|3dx`ZKBVX7H4{5iakAxnD#J=9igW@LS;HE_8$lZy1l|$wX zn<8-$u=7&li+^MB(1y~Mz7lj7?oYf%1k{wT#?(Mep094qqnPv7*OYkQ#7$pkU5U24 zzPLEwAb<VIp_uUE~+r5)jt(>>Bg48_{)twH$QJDSBrUS!j{lX z)SK$6dfLWt)c9%Cml+sRp*OHXB?e4hbYZQo!@=6 zBPTpi&6&atD*#Cn6f@5<>79Mq7o0^E!NH)bD26g}?@qg%*AYeE6Tec@F?y9Q8i}^s zz`)l`8>;h75!kL!`&*_hsX1%2)(lWr|7!}@gn%MfwY8vN0=pMm3WesCRv5e*5m4z|u(zbYCpuxO9$bY)hkL|}mRj{3dlRgNK)#PJp#vR=ka^TZ(tKVI<>M~ekIfd2 zm3UDUNW*ZvS5L|SF334|YD>LJk(EqgPpVxtzwclUNaH70zWDVt^1+cz|F?RdF4HHn z@4~Gs`lj!0dWi2n#>7C@B$Qf7|t{1!3mtrO1H7 zi{=I#^Oa1jJiFI!j>PualW+ncHJ)TelW$bv2MqUG1xK7R z%TsQfTn)7D3}XYU+{?Hq!I&fqi4>DmryMiO?!aN!T4fnwq2vsuB^s6fPW@u*h-JwG zNniJFR(RI*?5HV=tqO)lv}CRv_eNEBR%z}Vnftv0+DUH^OCODH#&;{+aw^1vR z-c~|Mk+o?j-^Z+rR4s z-gNA5guTuab7N`{Y@eT&)!xF8#AeetvQ6d!W4BlO;0#0TxS_( zMm-A-u+h7-PjmOQHlh{Hxn+J$jh?uEtc8RG8tu->og@ z86A%eUt+P8E3oLXIrq#K(nCF@L12>=DVT3ec6Vn=B^B;>D=O%op+0BT;T)FHZ`I93 z^5|bpJC_kB92`alM40Am>Yz5o1gxkIGRYQ)x^+R|TCK)r;Qyq6+~S9Uy9nr^nkvc- zxw~#_9eBBJcZNK0yFZxUK4h>u$8;4k-KpNTblRgS(y&u~u&J;O!aqAMYJp+(BED*d z^I#F7vPOEADj}Pziprs=a{%qgz#eso$j`At7pN~bDw%&ba-+4pI}T*?w-z^_~DfD~Z3Tg+#M#u{s&uRF^dr5RFZh7<|WNEG;P z-_SzXTbHc^yD$r;WJqqJkA7^(zN`nzQ5V16nG~Zobuy)a)(T@Ik>V!qOfw;e z)?AZXjzDJg%BkIEY&bm&BczLuWY~k}3Zyx#)jxg1A9R`sz!_dCb!|13b*3PiA@(E6 z9HmG2R>-YrW93UMQO}XE4loI(*er9J*wDUd1se!pzdpoB_v6^lQl}+!6e5MS`+bU#_b*a5Pkt;o+lOV4loyn2P z$3;z-cX>$R{6M4q%b}aMBF}6N+0RCE70bB;XwHV~JLO&!EB)Cgo9ta_>>Os1HNfaY z4PNu7BGhw`6}cm>glh6i^)Ja{rpLHix?C?u;(e&GI{?!E7$9hd*5c^iL?;6Kwn z@qbBE|3UMF|F$Ok>7YY?CeMzMes@CZJQ?&|R8v5M@XvW}jjxhjl`gzl;rvy6Nn9$K z;1TKGpUgZs`vR!t-sD~2ar{58-;2k`H(MIWr_cujtSCpjue(R z(a7R{q`G+;8qD8D1e?1zWv+pPFtk=k#>f`yqZo)3KwCBgABgQbq%hu4q}h+Bdyh?* z#Rlr*$38^Ru%m9FUTQL2Xy^j|f%*4H*{zWFRsMbs6@u{JM{48fq;F;QFV%6Dn!6X0 zEAr2G{RmY8;Jlmws#%7Hl_TvQMbLnN0KGK=9)1u=Vb&#V27UwM#U+)$hn#hlXxBxO zM~<3s(W;fe-0%mVWtZ)oN|h-01@5z=u(z!V>)I9-IepH|_q6NR_DA>2hxGKt-QX;H6(^FXwcBndi1s%qn2sH-rsuON7*ARP6Qt$2XIy3d#cn8sLh&7#USTFn3 zQm-o6-Bnofon2V;oq-v1@Ye@NuH$Z~+th}Cs>F7=H#=4PKLp%-!EwR&0`a}XL=br< zF>&?HNr}9ahB-EA7a({^_6`taBwmB~hJG)p>8r^vq0J_+o`sOq<{s2~2t}W&1f5`l zj;E0nmt?YRp{ONhti9{4&rvt5uoS0CO@%+Yv>+}ROQAGP3VLu^S4fe{ZRoGviEXMF zhM=I=Eg2~^5PIwEq{~Wt?inz13!axZU3knx_)Ey9<)z<=!TnCPHvs1l^spF`@INYQ zY|J1RWri-^D9mVY5Z{u+bXg#}3rUwSXX>&@PN+017W@!L5H8CvZf0wZxQ=UrHJ{Um z$Z;~3t6ARGql*O1^YY(h4awy!h_brE6&k9B&5l;ya>jDyW5?o$q~=1iV!t7#8&QOx6P zhQIm55sij*Ef-G_?k^$AjK2j?=QQ?^=r{MDaGZ7`Yo*Kp1uoZ=&5|O)D#xAHL)n9_l6-E!b zVV@8ny;`XU#X2((4cTmv5unmYzUmJ>Hm+Kvht&a+j3nr!sljTHUZn^0w@L|WKw2TO zRO>T!>jutIzNI5U_KL}vd00oi6$aJqPeJwq)lIr(2Gt#52i@sqCFaWC)pS$pYoRCK zd*$)r6FCClYp+n>gCqVF>x)ghAbl+h${~Mc_sQGk@+sR@b(88l zcx?*Usr}v|kV!RPfS%HK>Bn{7tdEV$CB5Z@=uy4>^(o(%@R|_7dq69s1(X_8szPZ! zSS~$LCX>-}F=io=YcY~9!vqo3&dh9_Mosio`zO6i|$&p;-9%+~sdYNrVE?Q8rS+eHx z4O$l|b3FUT#2jb(WU<`oKAjGQUsoCgE1(c>3byBNPhKeJ7f4S-hBRqRyePY)im;>H z)hyFuFTDqx*ZgXo$hn+u>TGs~=Bjqr3bhPmXG)v8){EU;N*58NKU5;EIZl z9%|JomX+b6M#jS2`B%~!+`EStMD{|y^P=`xPbD$o6;|!((h!+y%7Y{DuC!NCKDIN1 zER-J?vZ$2el4y~!-0vWjNRoC|ARB`IX@M&;?ZpULcAIu`zlH9 z&JK#H);Ij~fqoT{59}OI#ViA%!lPYyd@kHg*hyI;iMdCtw2&eLHOd1*N%2Y!BG*H_ zu@E?VbtZlI{7B{C>A^b3njh=KdF!=rQ!)oIjwkP{t^I{2q&emQ-C1&U&fPC_viACTbT;(A3qRJeGINz^!0N26vQ~o|#pmjp-Zq46%+{X9n zLGKqhLh4`-(*oDHqHU~-45_+pe(BICF$*0jD&FW?ED=vn=t?p9X(%AH9+;6NcJ8JF zASkf}LfT7Z3u*#i$ml`gKIS>3jrTla--x##EDM{w{>Iu9qV!x95ECU*W_O`q>hcCa zswU!;H3R{}(A6aQ(B)lImTF$BzF;$V_?It*+8ZeiZa|b8n_DN4jUfI0jIA6Q6*c0f(uq~DxrNm!$~G=Uz=qP*)?qc(}|7MQZT&B=Um zr{Lj_R7QJAlwD=CoYpjQsUyu1)C9p5CE)%3nb)~WtP;@6(qGG`*qDT zS(zM>&R<;Z23V|80%3s!`0QpTt0Ay;*xLJeE|DP5@x?a!1)`g= z-1}G_LxiiO(*?R*{(yH#&yl|Seyx6*+ETayQtv7Htk3WPvI;U!@h-e$)gw9>pyKmB zk8#$3BF-ou%=`9_3)Q`0ttk$cymvULFS`Khmjes=2(-QY@eVjJ)rSD)z)1No&o+dz zrGItPZ$QuD;Nqt~U{J?9VlM0g{kx!4$?!?=o?um>#7tjMzrLfv<@pI&cp*5H>XPPZ zu8Xh&6y7v0pGDiQqd-~tBjK%-SO8$8kG&44|{09|FO5BoNkV6~JX>g{b#NHJW?gmM# zhbcS|M9fDc44(seG%$hK#va#4YL98mddGDi2qr;@CeiWO!!`DrF<%=_^*3JgoZiSj zdEv30G5`7ex`XP4#6cG;AQ}(|>CcCTGiom^pc*j-Mz1_oGp4iP*>N125YeWCw#L4H z*>u2Ih8jVRJ?rOj-7KbU7KXpYs2UZf)Vf}(lsM(oiB>tgqX2tILJitw_x z&7gq;`b}qrL{lEA3DaXDOi~HQ!^?xxjjVW|#Z+Ek&GKA2dYgO@zB2V*eY zx>@D06X)(FUz3xz99V3v*k7x|wxiFxv>=N$1Chfp>CErJq)gnf=P!u-QKrYnulzdQ zP56u!AH2^QVnuxTJjcQtlflq>PSm4C!$^fv4V_XsIO2d=O8|J`4bUDtjBchJ!14~3 z#mgUPYF*Z?k;Y)Igdx3yQg8L)M=c%}p3!P-0KOuXI+{*LXJ&w)$gzxeTyr`)h-Nc! z`$xa<>T2pbuU0VR?#FPEM44XDRw+cM6U1R2aLQpGHX40=4Er=lp&2aN#P1IA3|r+L z?5jaRyCgN)b(KuS+(x9rPLLjY&4^YY{0T2Ai%`f0p}sG*R!}{DSf7GdPJ=C2MT1ND zUJ@#y06`CNc9n?13R2KY1K*SYeV87wG%bjcIbn+AR8*FS<{?wWomTT5@`}~z3bFAJ zLR-wmE$iwwJ-TnVEhl{{?+??DJ?DWk~VaX-L3-RLtprT2%z-GfD{UVBR~T}zymA0 z6VZ;1Qr%5q#+Oz#3)`D(%WVWWS4BW6%ZvAtt!u25FO@e{X`)_LH>p&pFzx(wvNEO- z!2$Z}`iynmY2j&UCmRNB)9Cn3MXRls&PFVHzkzr;)B^BCMY~6lYY>0rsKT zm4}RV`Q7tbn)Aseay%@-I6ZT~PBsO?D|>kG*%(PGo=|gZ#0zsmE})xxtAvaCe&$1? z(7GyH&^jm!cguuMo@CPA&-lrdE&Aq8GIOuUK9jt{K0ldcvJJp7I`ZMx-EYj$)hl~) zFM!U~HxgO+lb$1cIK-nvz<5OPs(@d4tB6DUa3?-bJ98|dv-kIdtMS;9BuLc{a~_wW zO$u`rNymsAeMH9zh(|w=<*V z&&B{&O0Am`<$iBa)>pNZ6cO`d^3B5%=gmsH(HYZw6!U(c@}#)19F}`BT+yOfamJY$ zYOmy2m^k+ADH2klhAJMLq;6>t3)NREUgk*cjJHg{NBkVhDORNK;v5362&NN=y*Ef- z$vxYTG5Ga{SI&C93^Gsu9G-osqbC9PbsC&@xxGlF?o{!rs9|YpEE?P8ix#yS`7JUy z%ez(_Q%I^RwPrW%rFF(+mE}rp#Wtg@^>O7T(@LFA7j{LNrL=XGDyB-|3<*mqLL_UA zUZz?ulF$5O59-WWZ!d@hRxC@4d6?okW%`1$#<5w9eh>4Cyr#xe5%VPG@TBe#HA^O} z1&q{T_TMTr($f<()ah%TXapiGp}`MAC7>0I=Cx*t+bXy+gMyk*#(A~ft=&4YBdQki zQ}I=c;etc@sD4?l`eYaksPtJnx5OUaZ6u;7p64DUuI`omrWjht5$8+cqb6Hw75WNX z@D(fl7tDl2H)H%QYyX3>cL0*DZPv8+ZgaP7+t_W}wr$(CZQHhO+qUig`^@>y%s1~j z6Y)pXii(P=SQS<4iS=aOnR(rqe#b*BR~GN+bMNQSnhcMHxhVf6D7_zYs}@oo$eK9sZig1_lH0|C z&<1W;8dh6lutS+|02t0VqRfh9R+%!~9YsQ>cw-uGi!YMSo?19?Sty(u{GRqmTx8Zv zLz|nph}CNn+4a~dDzMog(j+NForDvDjLwub!b;p@dLHSBO0kjaI0CPZ)8B2(HNL&A zdr8Pw@u(POF1J*groJ~!1|E(GmnR3L6`P*3C;v?R zDw-pBC=u%}<}P_);mn-_cE}am&b1_WlqnWVzFS;*NhwoOb%+#0nI|H*Bw6_0R(=Kj z;7@eEqYkW2OvWkoz|yY1gZAJw8=>KShthS*ANzYdDT61^AK)>0H%LV4q3}hw?bkA$ zF$tz;<5T59v0Zd$)unmJ{vu_7eGDP6+pe(H&n^3E)g^rB?pn?GT9l1gztAUpR*+Kvt=FE~M zq5rZM&9v>ww1mzrK)vx*0;;?tnqA@Q;FBC@$2~=gy#jW$bAJUNIl_YpT)``*9nnkV zF!&XBK8(PeQfnScH*JaYqy{1bN4MwF=&g2)`!Kuo165*d^1Sc_d{I4>6V=>74c%g4 zXE_M`b@syq%jQx9VRp@ba!rY|MRhr!S3bN!1RT}^I(2gXE`KT57Y;maGA&dHM#`4* zy%@6YB0A6Z^?fg!$4Gq0auM47(jE$Y4osH zhydBwQ-S~vMS7)hg;AC=MRf~AHZu|Ue*bk=ff`!Ol1%=|W-a+~l)QH04q^oeMZHj~ z8$8jQn(n1#O!_7sg1hi;{v%?nd&gK7tfN3I{A0j zcg`ISk^Ir4G=(SvV$v}DE(nE+%rgFkT%cu5VR0Qa^H4-xPC*7Y*+E8#xvyepS#xYE+FyIIi0|5$J%mKAB58%MgleT%Zx42e^L`TdA~Ips z=NvgHNpYZju?*J>oNcmd^(nFUc+-bu4*+9)qIwU^g?1_4-&-`uZm&f7F^1?@3IvJc{gnlh?no$E9jFIfJ8i+33;o-!b2hD@}}{o}J4{l{44v z3Cd{3Lj%9^E43SBXmIvwsA2_8sXgRu=4=H{j9R(fYcCzOXriTZ51l+HcXr@)^?rK* zmc89=w8MW+txdobBh`X4rMvY#vuv0GIEO67sgL}mIw$pNW6s8Fd=t z@58{pFs^Oz&g}CPr8EL~QyUjk&}1qyO4;-6m0MRd4J9T2r5_j+YdeKP%Q+jnWNdV| zUJLU&d%m|g&3B83R^8K^WM{0at+=9UdVAzTnL+CqdcT#($38|-fQ|BJbHY4vk=ANj zvX?ek_oYp6t8bQz-T){|-5OGrv`IGd?>X*h(s{MvQ{j>fZbx<^-)&(j8(N+z^sftB z;V$0+Wd0oUR^&)Q+2bHfLt#V~jZT$UPUbkd#vD#zZJ&huG+-;T%sU~ONA?a`Va|T%I0yd%0*Xr3>p#slVg7Y<6o&Bx856S zg;7Q>mCFF?xq_m}VG5`(0fIX(V=yvQ;xjpwNhrLFMui8xdBw2aFOvI3t6-NG3%+d= z>1un%A{1+tFrn2nu2%`-hiqYhXDga3%{ZVkC@ROtTcA;g*E@K4i_G1&^P#Pl_9*m& zwBVKqZhrf4bhw@M)78cm zBMB!;A)H{6h6AjEv&|DGxYRmY|e_ARf_dMIvm*-i4hR#IU_#A_QYP@L|sHs zo@Ky_Bx6e2??_k;7vjibD#pM*T7`h9V&s(moOn_x^N|9{gkOtFY~gDqSo+7meUjBR zK2jiOsA%PwD|1*KC^m(-WZ5j2AWi;81kCi5t)KouHKt|R6m{m!!n|4YN3yyBo0mSZ zN^yj9>I9Y6dI&$!T7&$%3Ccxua0-&DoNJFbCV%1;h^-U&1Q+@47qrKld+QNGOrh{a z27PfD|L06XuL1+ZMc{_7rB7bd&WD%*lbypj>|K|<#2#t+qPXH zTm`5QC)ktLW5+G&4lhvX8DgOK)|mvQ_b^HuJ&=wP%Z6%;E+Bx|#|Q}vOoGR(jK}sD zk9x4A-V%Hs#G>J5XldT-W&|Kv(!mEi;J38jdK>L|Q7~<_no&|~Fdc~yhC~%VqQc2e z2|pva(YaxgaE`xa5=u=WkhtI|f`XRHhA6|>1`)hDgYzt9kByS$l*OQ2O-a#Iq%SLz zV^&-mn{^KrM6&BueyiV}>&)9rr)de2+DkV8##PSmko(<`nqPVr^n_V~UoIi`_yVdB zzcj4`b5QijKNrR%0AYi<`{NDb!y1^#Pv|K2N8<&wlO7-JDa5Yp?eM)pf>PbMq@)Wr zvki0Y1yLr2WfDb`RBPgq^VC(KH;ofR#9^i$TaMi9J6p5TP5F8<&ofnvL|`*(;urRO z?0k?7WiOd&^v);ux~R9Hznc3moOxE+O$lYV0Ku|hENFV~?Lt!QZlMNp1%d#^Rv!pC zfq`*V)n<`Io8N2XGBOjLYB}#{g#>o-?Hmb6$VyvSN@nI?3{y-pdNvcYe%&%CIeh?s zWfdM@$o~R)P|M>ElHW0BAMI=ozdH-Fle#Dvq-bpmPg-!rDY|1*o|1dvDh9{`{gt%n zFemDyrWMrywXJ+rV5r%UR~0T*75`i&rM4=%7}ulJyHu{rZw;C$r+nn@cLyLgh0d-A z(3SS5tW>ZK0in8bOH$vW>HIcipgUXYGUq49#>Ixff27cCfWz$0vR4Dmq}CBw<~4Sh zDe9adM$vVItE_)3FJT5Bgk}V=1g+Qvf5+hpxwh78gHe$<|r1^Nh?B&_~xSq+nVdY+~dc4GJ?e5EpV zXs-H~6poV`Kh5kok2qSUMD?0&WXKs7T0?Z-J8zti^WD-*_fo zhAqM(p+l2*(|b>aZC+?aK~^_VCZkP0>}TxdEC-KcmAx*YS?wTK?cW>PjS+NxM==Wg zg}e_*NcH%2(J=+WVL+;P)kz0c@48^4ZuemowCO=rriJFSD|#7D2oO{}$kCbL0#0%2 zQe&D2wwJ3%d|+L`bE=&9k_~(BOe$ZFap$YMGL$&$D0=mJ9n%He#RRlC3f=|WyrI0L zA_qS=kzzw8f_QiJYg_b?xA6UgBS0tT_Y$!9>(J-Q|m=O+8+wIPlb5i=-aU~kBf=4dD zd6Q8*EoKqRCcMNO5q%nez-osz1XT6PZ+r7r7A_{!vpDIfE$$yCUU66H>HOUO>u7aE zs*>|KS24COy<^3O^xXssCI`2iF%;A&7{j1UDk9dvv< zsUbj2HMoFr%{j!bRrmyt%jM|4UKza#}%Vf*_fEvi$*6J-h}oRdsdinr_W1-)p24zB*p9tfDdUa27+yi5W`#8+~eE_NyvNZgCP48jF8P; zgYS#IP!@sLe^SeCy4jwre}sC*A4Vk3|EzFISR4QEai+j{bL%-B#Nlt4WJN3eh+Uo) zVtaBF&A%PtbaaH`A~$h0I(5#|WARn>4Hbxy+Jn-$LdJWL+&({?oGdxCC?@gw`D44O zZ)fV$Yi@4u-zGU|!cfh6Eq?2C3Nn%TL2ZoA1+5g5O#q6$QGS|1C!;H{)PU?dDlSGU zLGKxOa;zm!C-Zghet4U7l(%LaEQnKF+>ECNt@`F07q-JO?%%X~*k}Yndc#f*iq0`hgW#iOvymYI0Ur}T;8qZ+%f1paM#v7e! zUS~+CMQqEbYZ%Ix+4iKAGa>>DLya7d_5zQo_zm&bP6F_75Qk^L7A%?p74r#_+3V6R z@m)%h$SZlQi)PpLLYyya^FulLkrPuM%+!YnWBCX|f#M*ph-`6S5IH3F;Os;ZZ&cDq z<~WF?be7SQre3OHq63A%t27ee4>e--Q*N)lFkAI_P@Yoq?Bd0s)IIqLY)xtXU`k>x zfQK0;b2n0v{oPhQju4$`uD>)Syw=X_l}YEfVF8)awhULL-sJNdq;z8~(wyAEW&sDx zxqHk8ufaTXHNnIUP~eE&k>D!g#IVt73wHY+ugJwtuy74u* z1qC32jRV4EWbz*0B5d5qGm7FB;V0Z>C63g4n6hW?!BfHU=hqZbuGx&ccdij#|lWok>4#{m^Fy>{`JdOS zjIM(Tuf4sYrJltP%2vW!U)Mt5hd5_vs^{onYW=T{?nF6taSUF>uPLMY@>8Y#vd&fU zJg$MqI>EOkIj}Gpu%?+k{%zvX7zqvMeuMm%YD6eLoHxL?e6eW>J~|~Z&lHB^r_Ag0 z{*SlMeG(r}i;4UY6e1TDhAnY@tyh=*e7>7?vlwq>&py69o*=hIE389P!iE)Fe1v;HN5fVGS&&jBzQk*Q}Rb%{FF5H zt;vL@*J)TU^_AGy%>+&9)+R@9XQHe9%Cr#w>Q$NM0~WAiktZl>9`I-Ypc0UjVU1rn z_FPNg@88w2iz;NHBJ8)vM$%1oe7QzSs;NxSieG5h->Cq6`M#YqU;tx=1hYym@h%fi zzWLOcEgsbZ>jW|mkR)qpxv-Z}J6iTzy?L3sZiv!nbZ3a;A~Hu3j6-^%FcrouBW^*9 zwOO;eD$2J8edza=ZDF&}5X#=B9O(;A4zyM&5yTvxuoqjP+FZY!ZYI`_D=;czTJF-e z1-$=(BE%9~*+c%p5UT&+n27&>tc8D77L`o(F_e)w^~KRuv4^AdNE-D~2I(p(SCPRP zc{V^gm}JdYd(~~{max0nhdPp5j3){eJ z$LuzR9V>9)451K&?27Aps3vsd_bU(1EDOA~g;@vOO2Ty`4MFO9u=`!_wEKPQp>9L& zzuUbCBGHhsuxYBy-^Uw`)=n5pSF5)!a6qfH$^u&=0GA(}B-Ixjj|ce?Bp(~$q^7BqWU|H8 zKU!?5P@+8*_63=^7)|h<=`vW)2%PZF(`Q0Lr0x5QLjWKIQZB9)OOB_ISy!Mx`E{lJ z1=1d&Ic*{{_h#6sNH^Hz)~vB7gCTbuUkVrOm(pCye57-0NUsKiFMeA#@NBB+F5<+s{(H7mQAPQx`OR z8xRz&uf&f&-?8paW&Q%EHCq$Lv~}lCIW%s>Wxj&$Majn9D~*{Yn8jBZ3b9-fuz!82Hn?&ZI2_JZYAy$kb_?7m*?J z7EcrbL2*)gJ(Wl`yg~c)vC1w>dR$LezB90-T0%EZo|KuQOirNpKJAd) zr+w2F#9m@j64vevMEx_$M}ESx!oajKsI7|Q#c-fWRsS7nAgMlxf$l`eoBx6_u1LP` z5wVEEAYNPN*iXKJza7=aP+z_r$z;5})SQGWl0SrU7qL5T>MpzjZPVq~an6pv29s{gIn1Rh z$*Vp>0p=05JN|HRiyOCbpgpZ@;9Xj|o3DNV!%Xn6t3hE>(=2$dFuEx{osGXYv`m73 z@j>86*-gsSS^3mR)HB6Bj1fy+E{@9e{bcRLU_iAqDzdQUqG)+sqNE`h1 z$3w4loJ+!{F4NdK!E7Vu6L}j5d=VnffP!j5b(b5(u}{;?o9PB`YLsrEsOeE8IUM8F zj!}~kYF^$l^i7CS$AnS+a4#EnWySE!?hNnzWe>=ETyc4WCXpNzZ9R&vLWR9n2)aFS zeT`FE>ZzLpjPr*qdk%A3<`U8cpr3K~?abpqM})l-j}Hz+9tJcw;_-BzCtzpYoNVk^ zd4xI@9~_|+Y_6S*Kx+?A$c)OqC718Wiat0Sl%qFMhix0?j{gw1XO9$zQhjjoeDj|S z8hS*$R7Ol=9=Sd-9s*OgZAC1sMC*(iexn}3CMYJdNZu8^S5)5@Bxo7ayS4fG2D@ns z(Y9t_4DB(20CAx~=eL=RM?RRc4|4V{?Qe z=>g3K7H^2nxwHm|*N+zhk9ET-=0ak5wZAxM<)DFY7|^q+@a_=>AXMj@vZG11mH%nQ zn9XfRt7)!V&u0~v+`DaED;5~WX_cQ6~@iQ$)`#bKdk&+uvYtZMGQ??&zRmpw zbc5donS&q;jPQE_7rh5{ONJKBM;cxKH>r!f)K=VDf}bfc1B4Nv3C}__D{B|kU4Q04E((6!W^q+&Xb=m`c#S!$wEEp4py_0 zDJO?v%A16hzF;#-Lt+DUyec?VXUS?%21=wBiJ<}TTQMa&n$+5wnHr4sni_Hb`tFO; z((Kg?Xh0p)JZnUc=-mE(Ls`z5)+Qr8;F0R92sj9yEJx1kK&wQ8S2S`)h+Qk?^jShBw0n z^g^Pht7xCZvs&|5W95{bypf4acXhX`O_>*QyEk183j48^Ws>JcasVrhs5G9;&2dyi z%>jCf;J1W^x5i(=Cvt|^PAWSdNG}XTJ@;UD+R!_#xn5!VD8@`C$I>Ipes@q*x>0`l z)z8=i*VF~+bxTYjaCr)lzaDau^|9V&q!IlGwQu0TKbn4oBljDL$D`d(xUR1D_M2H5 z_D)E{)YMOgPe9j&Ta=X`w!K8L8Fz1tOon!uWan9)huounS4Mh4dF)BRXPW~rZ){=b z8GKrX8h<5U_7;gkNu2?Vha=mHR?g_-tDJ7e(~;kBqw^DncZb0-heR1$Eu84i7(X`&aR*AQIwovW z>fz)N@L0uBeI%!;>fF*(y?aB?LspSl*h;#V3|hH@lSBCC>z%=##r4vBD?~% zIcaMD#Ep&MMR|QloYSVm4m`6&D~o=K)KUR!2dn`e7}AFYi4ni=M| zwlXp`cKoTc{O?pVGTu@effshzIQL;~Uran3$O8b$6lS*o0sT!BoyZd(zz&P7axA%@Nz)_qI zkD$LWxQoOtM=CJA^aux0eMxT|$TTV{XcUf%R6YWWWpb~~Wr+7tk~!$o(-O!M!{#H? z)jCw2taNz0WO)=*Gud3!7Hi9?DqB;9JQ_pLDASj_PC!c^M|om%q>Zz+S3oK5Y^V&l+!?6vHO@6@c? z%)vqVE`pRD|ItbFC1kt4ApdNC)&9im8NW=RUr>

@up^y4&I8N>~wvL%f(S2W%NN zf&x46sN${5Gh+I9cd>g-O|x3@x#@hdvU54zx*WtnC#5%quWk43w{;_G!4&;N;wy-O z?urjbDnKfp2u4gknf&*wBJS`YfdzBa#pf^Lo9ei}Z)MCk6MP}h0OYrd8`jVipqsRTq}lh>h#|o4yiA zbPQLKXatZ+L=I$?XEGfd7x*_lf|=3xKLi)yj}jQ9pD+OPrv;Mqe+~uywe$sD4D}uV z4@_J6*&E>)?K_L=^f9)ZpbIb0tyI>qF^OuZ;8LrA_T9JRowWUXNjyBVFxj7 zcFv)I!ZI!9%3&ro1=#}qZ!W@`!*%Do@xlC)>lS-KJPYY3@3mXj^ZUgyXXo8DiZ)0M z@ORv8NQ5xIiv%yy7WuvM3l7ZnaX8M-u4s`LZ2-*e2V%BIin4U@4b=3ps|#~L^v#DXv3GDk8H#;lK%qAV<%I5Z8dd3-sIMfqq2WY52;$Y7| zC@8Z_G%EJ3tOhCq_Ad3l4=IN9=Ee$7k#R%^@JPd7SnqL~*a3EWdfPj^Ft)B}bgnkr zBT1I)!g2ha@JU#wQW1op@1SkuaGVJcEJVhstebVvoHV+n`EI?;^p~M~tfk#K1CBi- zF<+3FQvDXkoVE)E6Bj9T)Vlo9rjgCj>S}EH&DnJgn49L@7ZaI=v&F?OY*>NLOQ-u43cR-0P{LGZCyKsW{^hNC8iDiqJ{~) zNqU!S?7Gb=jXSc_T>xTosLbq!#)VKVs^hKlReb|!_v(O0B(=A8tA0Fic+K)>Lc!(J zge-eb*cuWjJCE_q)D}kLQ`X73XAD=didg`EDAk|uw*rjJ1Yj*bj<;`v&pOnps=(g<^CaeJRd*q!NQ`O zTAcA*KCphxtD>M<0l)OpWo@|W=Vs)XFpM7C;96VQR+W3~AXoqC9@yN@7J9kuboR-H zHL8|U?V*D#Jg&`hR95a1#ByH}mfw|kcIP#b2%C}r_nxhIoWdo%k*DB;N)%#~P458H zR&1-?mh?}HxGi(-dh@nkK_H45IB{y)%qwup^p85vZeUpqh|G;9wr%q$_*4*|PS(bw z3$<2M;y;*(WAtHSM--PRyA1<)1Xe^(yuRRaZX9nR0oP5%Wg)P(ak|_q$^7Cd)NP#f zFt*;;hP)je2EkvO_Juc*@6Fd}(xbH@+`c?h1(9yjJzcLY^!{hs3;2?q^IfrF`+D{7 zeAjrrb~tUbxms|met4=I%jCVN6O3DEeY8_%NiNb1EvTu>AI1J!n@36jd$2##c}B>0 z4L;|^v$`6=K#^tk;MTA+ji{smQT)gaODj-((|WI%X2JbpJ46#0RZ&FMJeh+Z<&>04 z)cI;7Dm)CZ1Q9H0Ge@zDXKAsB9dZbg4?1joh3}_)K2k;c^(s6)kl-$}hLll_T0$(y z-4SgpruNv#}%R(l@3!%tj5l!d~Np>{BXo}gF5QWAP7*n?JW-N~>|I~-Sokci&_Ho87f;meu+(2@Yz45X{^W92m`3_^%9FadE5^cGO72ffn`$&G} zGOIPIF?FsLh^0eater8)<@~LjNIyP(W7F~ackhd7ase+Gfo@-RBG6$Q+CeDbE-eiO! z66k;0^Ze3P9kEj(yiZ!_vx)K5>+Jrl2af_iKMbiG*Z6y})9{?`w@LyvBpEEC99HEm z94J&4%248p>c%Nb+Y?Mm9%w8P;5(?F8nINf&_*-><^LeQ6{hj_UPeUhLmtxd+Vmgt zX+WF*G|x;d1!gF0D5?$*b6|tDV#m<_?(f{b+Jd?J92?)y8t>gZ+-KQ+Bj*PJW__xR zdf03Su)GBsi{L~F7m?zTiiu`Wk!YO=QO{H#)PP2?loJ6bfRs0oKxO3+aYm9`#}5V$ z`x646$5C08JvW-c>mV&jy+a+V^zH9IQ#Inj?BmB?I0~jhx7qLD!cSQ9{<) zCB(xvh>|7z&?P1A6fTeZ=vH4`HaRJenyQMrBMl$uNuOX#!uWTr0YsU$pvq9H4wY>t zl^X-E=|ppy073iT6Xv?zU&~*SOz)S{s$uTKR(W@_aAsUm!9UD9D`~`uK!3`Buc{%2B4{J%ioRlMx&#kB{e!Avb zJrlj#<)~p=4r6CfO9_3Cn1xhg=x7nk+LY}yn%fvBEBY;q4p`CSxj7WfX^CU5+@tJWJi(W&KcO*jj5x;xDLZ*AxFvIAYA@P8yW`o)9#pos(U zSgS*I-N9vd=^11lccI*yNQxzMgJ!_I?64MNHZL9-U_DIfm>8g{k^fj)WeFHM8I_z& zZ3l@3<|n0jQSo~R0*Qcqvf~?+vNohOl*bzy=)XeN;2a3p1~0V$$gAWoVuI=*iPkyO z;E~luur&+0{@(mshrT+g9pcf!^T48w$vch$Nigsv6ylw&q=E-ICa#nDgi$8vmBC($ z=yLuLM0U-^2^S`{_ZwTz$|kB|ZzUr`AM@J;{X1nZJEj`$4skl+fss?6#-GZt`JdU# zvVUW}%8!tF0rBe>`+r}#|FsnVkBs^MUX+ze>dHSpWnWVCqdl~T@Zci3NHq%q1q0&Z zjiRz*rIA75MSd&j>=Hq=uts|mK)cc}S884FYT9`Ym2Gbq-?zNU&7M-!u<)j1^s21K z7oJaB$L#M;cjw#E-oI~{yJTr2o((;6binRCTJm*%J0nrPf%?1jgigQI5bI~2dsFN451~NyCYYvfVfu5!YwE`!Uv%`& zB-2spw{|p}vcNP<;@k3}sV|3_r|H|Z4JC9~&KtI*)@JhM?U=mg#m3PjRVoE+M zVYM5uWSO==K5bE81EEz2?F$jdRB^ec45FWK&Dz+e}E=Op=h#{z^;qey2Dx+2Q2qzwA-MpAB% z6U&685w0+}tjouEmcVXOF$U)7w=8u*B7piVzASTr-X|xfrQR1uvc@IZr$CD4MUVF| zMre!R*v|cBT}rB>9#r~c4@(}lBCp$9)X`O$7f_9s)8|{>$Da!Go_qr=;4rtnr7TgXUpffMV9akHEvEw*Z&g!2Env6(!b;)$Zkq!j9UGy>Zopi zUQ<$5Ex<;BxM?&1+E#8>B$er2c?TqH!q^=LX)1lV=@=!xtMbm`$gt70@|} z8AM$V_n1o@=*E15EncO@{DFc)hEBSA@Nbk=GkNsF#}_mBtmF20k$-)eOP+G`q*EAP^>>5d@ea zg6^gb37{ol+=uYC3->5=jbqd}&J|19Oh}yYviQ}E@&>94`r85c>mo=XKA{q~2C*8q z1(8IqD#!fuWdW8DT^RfX)ssdyOzHq^sC=mmY``qcE8^g-o852h1`FBL)_0fHqqzW%Y(brO+X5H!1sl*7|2>*^XZQ^Um1qp- zj{+=uY~SxwTj1)2rmt7luK=kSptJDqqF#W3sech+R{=RBs5U1mcd@_EU~~8?dsmUjsf7tKBg%yZYVwFEDFu zWWQwnb~$%v)IaYXT;h~afPZz{4^@br zn($GS68Obz0BZLqKb0MyvEEp-F z%XZOu9nt29ll>hIY!o7Ulpi znv6Q&d-;x1Q#smNV37IAjmqJ`f>4;j)zs}@5Ggb8NHQ&r9}YcFk1=s0qSmfDIT zL}IzQfY+Hb7z3YWw>3^;vPtIw+@lL;+6f0j=R`K1?Rs$3&Ft1)@NM5zV1L&`Vbl&7 zswRx&Edg?U7fqYMBpWQ6jO&vI*KI5odc0(9&B?LUS$lNhs$&T-QLab-p|8suK`a9N zU;>Q)dneC-M2!FT|4RScQqNRUcScY|-Hb2FWK7ixX)w*zIKVgM!)R>CsoYSb9@Lsy zLJk9)H;@1=N~KM;fxCA80PT1w>bSwB_El6JKa7XzdPVs_qfTy_HegHLC>RgUxX-lj zs_$O^k~(_!_WADl_zRBtc0-mj? zs$_XlVRk8UA;TzI%p`NZo^_F0EiGU(u~@&bF!!jgly!a1es#9LBez7Usio}j;#J*M zYwchj{qF*wFL`?T^AP-=5n(>kT+$T_0iGHp4PM3Z+@Rs&k(ghDz;|7e>IBW%Q&>Q* z*|!8m`k0#8(2SfZzjS1JdAS)iL*a3Q>Tt-uHB0^>6;1Ac&)lXvA#A+^~TF&^<-Px{Arzw?$8;b z6(xcC)ary#!{#M(-LV!}WvwJ94Y}p+dl+)^9$xeZPD9+g#b-y4E)=6{dZvMSy(4bs zQqd@m1o^6YxMp0{hxGGmxj9Cv;|d+QcXE|*vQbI!0Pil2SOuAXlwDZl!rN-01kujv z`f06S5M~gsjn6G_ql(Z9v;Hz>hvm)t+G*Reo}Oz2DoZC~IJYFxV3=*1bcDI#V-ehb z`yS4?O;M_uUKUWRm9-0*%jA%+L}L(ouJ)NW*6>k4H0cLNq(fNgHv4Jnoecj0zTR!} zd#20Z0rVivt#5;(=aRdjZc}W37m&` zO8hf+O$5W$AK*8A8`$z*=vRHy=*QmoFlAg=(s#RhNTHVYC1}1K@hC|GVLZ=F6-*0x z{+sO$vPen^=y*Dt6A!PzJ!}(6LIqT()R5jys9m(YH-ka(Nn?~~Rtl-H*pP{zU-MQ? zlXus*&2qLymA^@KO>Y@ZjhbR)e1(|kVQ~2STn}zH$Hv*3wWt5KBjg$eN#@{G$fcMS8-`5K^IA7m_aM6 z`$)$n`bVh3x<&!)d?X1WLQ9uG9!?;qPGiS*BaH;RE}RifZm9eNEHWtim)l0DD^SyZww8iac z7r6e^#bzT+IQYWSF&Kq!LAalh*r_;Wzi*>jtu~LuXq%d^sr49_?y34lr!u2w+EXxL ztvGKYoa^y*IC%Ypz%YnJV8{reNW^fpBHc9m`O*l>0iqm+au0Ze=X^~VrnQF?&PU+5 zvDnPzI3)KOpigkw6k+Ys(1~ggta{l}hmoJQoMZf-VJ+IOf#vtk(!25;+d@FGwm{aR zAx2bT?D_&PU}I*Rt}$?_UtrnE;npz+3Wm#cQDminaPZX-ZsD&rZgNMlOP>~lPs)5- z1VY9g@uu8tU)@>Vy33Lo9Nkp)j+fdu6g^!Frwn87+^Rz~KEqIZNvGPU)wR*jLB$B}I$TO*f~!7t4654oLO6t8V2r?1+T_Q&0K0 z4682u*_{u6j(?P@{;`Y5=-T~Y%Kr<77Z}0&gZ+aQ{5EN9gm5}+3o-ZC$|VI0^CJnl zlu@4piaXoYaQOv8RMg_I3w0k1bN&6lEJ=n~1W@$^LZ*+5?6;J{!0RU%BNqm{<~-t- zYBiVcsKMtWrxI-wsbMy>B;oLhCnBi?O$~EZ4$9!UcL&30S4}6G<>y$P0t(I%#Lna} zX_$_w@IIB}3veH9GP|^0P;_>@eR7vav@g)kd8j3{^_~v_K#JRObGNy!PKV z%zyngxUd z^s@D@xs>D?9|0^XQSe9+5fMBr9-1rL2ipylxZmKI{+KWoVU3B__h9-y+tCNq0iyqW8C?N<_=wTWv36hc-;u6_5$-8<-iG^wVX{rs#%*o<0 zP`zZD%9FKz8kA)Pi`QrR2c(!`3^|x4*s*D2BB*E3p1pCB6wSJ(K~r=?GY2zKWbkSM zk97>~}>cv zb$Jz&BN$J`J1%`SPSlD!*ydwZh|}u@DspA$4$sz zuve=&^SCLUwSd_bGS|G?7q|}mlM8;PN?3s*Qn`LoL_I|_0v+g4G5lm(&>D&~sR6?l znI)Ws=bL^}57Jk}tm&JypgNPrn=57ljDoPx5vC%_rIdlHBI-9tCQd3ccs7 z8t-*ywH72aUrR7)OSDPqV2JeQ%}`Fj)8^<7+S({A|0d~}AU_#mFK*xIuPXctHbR_6 z0>4#tdv;L;zy3>@ngEyuC~{UEld$Xby%R!P6GeG0aQ`p@>*JR7p_5+YHPKN^V4fk3 zP=|o0bY4goP@xf7HieU5*Pudrp}QZK@B~{n6cMl7DMdWz@t^;~@D^eU<>!6(45Z(_ zk$+hp^uOOo|9MRR!MG0pHBKn;ANR0%BC@7!gZmJPZJXt>$m&mX8a!}cI&=T z^1$X1PVvlD`DVXD#eo%T9Hq`v^hcCB+%v=fj3To3%ZWn%=JZC_ zoex%j4J+ zbQX)n1VtYQf2U6; zl+lO7)ctA65@v(JWy3f!Jhj+syx9tcQ)P2qi3?*W-Zw#Ork|#Fs{k`fVV_!Mn!xL3 zIk}JIQwGd7Ve?#cLD_l3;B&IP`k1Ad;eT4RS=pW5A1i9B3J!lo3 z!WN4Denb)1o>9tu9*MQeIgR3$ z0rD%TiSRC-!526-Q_<1bGYn58#9j%95VT-muFHVK2w+EN#G8i;i`sA@UJgGpB~}7x zXT$xV`dKsMX!X;9Ku-Kvd`_&(SCYV;p<-2TVNbPS!mBJ-Wd&_+BDCO7!-ztt23Z4X=cs@kswD@}xU^1g^h~pu=^6pW ze8CszeDle6mmn7p6^EWdfD|dyNB$Hf%@?7eA4}|ajD2dyBKnD5ou30#)271<>qDF}GnvD)t$ z2fj&M*=&%VGF>YIAwtb!y?Ie|YWR?x(XuT5a+5#3i=W?qc_A~KjWxnJccu=Xz$PiiuHzL7#&Jt#VEx6v~-8J%V@+^q|MYi z{c+eNd4k(vCCT3b1G%D0UknFNZ?%lsqRm{_Bk#15n|;|H)9O&HOroVE-FG(hc4&ZE z(2P$V`Y^c7#KE)tx3Id<0tT%cp7~`AFs#cqf_JH!mS_Fm3^W1T!JXma96S=IrQy{} zb0%%7OB-G)J8g)5WpUWTd10Kg^gMRt${vh%)nB};`vmNAbL>TCRA6}wIE<1qWykbg zPcCUTMV-!d>owCDM3^BD{hCpJcQE*pH$gV#ErC;Wx|Pm9SnipSi4GEzX%cltZ8sf0 z4GJEGTyuxoh}YL_^g{rSCj(Mn9xB&ZpEqiyz-a5H?)=3b8E8s zNV4xhy4dT&cqJb_1$w&<_Ly*)afAyxX!#R8gU)gG)(#SXrbXZnoP4uq5;X(XFv+a6 zX>3lBn@9^3=&!a@Iy7C*kVuccxvO@qV6GM z%IEWSgV;mL3SA>lp*KOzvB5IVgDpwgX_;?gI5YK6==zNjtGgy=}3pI7Ml z*K=k&-d*&zJ{n?u+*PW8qBhLLy>UlMZiEIK|oHw$2rs9WFwD^(_d8L4@aT5=s?a8c%PT*VUVg&tO4QDy2SY zjm2bF%vg0dwTFqL)$eqaDox6HxHo5b zNFgp5r*h$E+lpT*h%KuH+&3V2#-tv2SyzkL$JGiwZeF>fbV(hQ2BwSr_!rt3?1T{# z3+p)Tl>z*Z!>MQQ>u0C#>Grq9WuFghUm2<38IZ<^qz{5X#CQaF zf*+9#(YJ9s#v$mL$-q)RasrGY`j8?J&3!QZLlA<|;QEREfPSG;1T6Zobq2^_0kt5q z09VRDG;Z8JCf6j{ENFc;@3BBW=)L0zw=Nv`9rTWlU%SG*pCtHSWjNhK_eeShOUWc1 zguBW=S8?nd=TBUyH^szUGwHcZ_085TFwz#|m8>-DLDz_i63t}Q{&1Hz4#&BBM00Rg zVBLmTo3$&AFIBXyzJFV$-LXKdTj9!w1s4u$sTtwJ%L#eIW7Q-qMV*+xeM-%y0(?Xu zYf$T);aSqS%JCFk#=-}_oMlbLI6SL(vsS@VW3P{axttW?Aj^|nTNjt{WwB<@*PDZT z83dbE=PjR;JkTlb_0}gc$vw%DL8IuHL48?t7bk-p_2$2S%@_`iYL2H6r(tbXtG6$H zi1#UpOr)gY$kAjz^D_2qA(d?Drx*fE7ciOz|S65GQ?@VtM-pB2z zI4+D&hV8ICIAo>$0u9M+c}S*w#r~(Y`X!*Ot*s<>_$|Jy`Jtq%-UyXuOq-?62R=8(;>I?z9KdCKML;#{YLY$;T>XZm?=UMn_|2rJTDP1Hb8tg|jxd^v+7b=!NmtTqBeh&ZS#8&>3NHz5w>{Y4R_ zO^gPq`R-cbRMDwPNbP_#R>)zaj_`d(XF|e#kUT~iLdsnipk{POw`}Y61ZAD0nZ%DK z`9$<-)~~Drk;!X=k_bh1nq3~u>-~rbzMYZ?_?z4aK6~P}R|Rp=V)u!VrbLFxIW+2b z>QCbRY0tN4TkELh&c0Z?EZk3qPr_Z~pM`RmqbUOkJ-FMoK2VOdHC4y-G}8eV+DZWk zX6jN-&=s0$n)ykYm32Cz^-9AHW)kRCfBXP_Rx{TG3mN7#g=+BS3*~Hwshl1}_t0Tr z@>%){i8cncHw7ld83d}Tbd$lY)kp&6w=djR4OnT|iOe!>@!}5DO!8*$5^bG9=g)2C zhntFe*FYJuTv6y}J@zbU^Oo(_A470wLp;z+iI}Hu+#FvD9GC*|JoXx#vUsEWFMWzs zrZu`29dr4^OWAsvC}BUpF4b3865d`bCI=`twM+)7OHA!s+~FKJo5g*Z3)bGBekB6l z{^OH$w2KEi*_gGoh!}k-;;t>d zONzdN&YtPqo8~CDbOb*JqmAK3!_<^zKpEMCm1_Aw;5Ap z5mLu5wB~x0{)K=s#@QHe4QB^QHDEk8EK5WS~XtNf1f;f+>NG|?7@i{z{;oEixJ8NF5> zqrFoEMY^>gJf2r0h7)7!AZa0;Q)Gm-_udiHd6-r+nLkdP8Idjb7YZHg0a|P*pi7*?SHZmWTU_)ek9rzu5jNMxZ1-PQ*8;dpg0KMZ+ zvg<$xcKwT1PCU?+SNM$wAHJ2tf2-A$Hg|CNMu7i3u;2Rm|Lb+l{H9sv<-UiSxL|KC zp<+^oL`w;+0@uOD5|ltr1!It<>CyM9qAyLPU7^`<<=sZwJj}lcAO#Jed;j1|xZP-) z_$diC9(R?o{+&~-z0B_J_6ANFjEe%X=ZqU66Q?A1(h!AWTU?EZ3$shuPcfd!pqaK8 z!fD0;=)T-Z(rPPKxoI++8v5w=@#2 zMjXbSXl5Z|#_JGO8fUn|tFn|N+D7@TQwqfCT14gR8eKfo(XD8)29;&w))lNX3C4^C z4_yvO`*Vokel4~CYWw|m?mdP`6}1AN$VtBqzG;7rd!*;vK*TA97s|PqHCZ{xFnm)~ z9s2x4@urFRS56_BvH!qM3*$k#n1pR|IB6|zmWY+93=<3xqmsN1=9s}qAI$)aN{!JH zA_;b-#~mdM`1_d@qW?<#VVuI_28>DS-W;HRhS3j+m07d#0Xp|#ZnIhhr8t)5s_EE` zT3JNF4UnQUH9EOWEO^G^5&wflY#veqIXg;kE-My3<3l<9gfNQkP1q**CvbxQNd9i4 z?}rC`rg%nf{cI18sklEK1$F*5M?}!fAVS$8bbE-G#XWNyeA8y{>>3X2v0d-+Oj2Nm zDM~hDkKQMEUONW4)V08yH^lSkurW|St2O-qg*X|7z@2eK@Q#PRzc^?S&VF!iHkZ9r zQ|_p96s8ueJgP3de8T?u*X4X7*PB1c+u43Z4}DJ|zhVoT0A8Fiv)KyX%2cjV8ZN3c ztL25YZ~Q;dWu@}E_5AmW*7O3qy%ypGR;@9T0t)F($+h1UowgLH!l=2w zK!qu7u!lkB2db9ff@F80U3Y&HLxo6uuR{t-k=~4>KaMap`91+%-=X4x zPIjb`(iwV6mt`gQh|&>5t)M7K(0ED|DJt@k5JMGy`CcbL;4X9eMpYv9y3t4yjy&B0 zXf?}(|7;DEY^&|$+8O=?lHh`ed24Gb-U*!6TTaZ0@pw}Q7YzJ;?~UHyTPQ)J#Zvh? z@zWJEmhvLkp>o(em;{^vHcBnExu;CTR9eB;(I!)lr!hG6E{)ZFyun7Nb=JW@0qs@d zEkQlh4xOnd+KSSjO@HD@I=o=|<+>iix{rdun$Lsk$f(=9m_IWJCWN&~H&6?b*q;D~ z_z1*N#2($~+O|WY^B2XDwT~$_Z>S36GLjfaX(W-3%cth0B?O@ffccd9nP^2UYXi03 z4uGbbTuq5S1&7(wk?e{h zVAQ9y(!U+Xu-73g-D=uy!XCaY0}{*g46Aw(uj3Y^`bK2@ecVX7t+Z{Sba#VZYI$;U za)t(vXQ(p)x&2Z1>e|kteyh;gzRHrGHZFI%Py~Mt0qoEdxHKWd^)3)GmjLTWKW3do zAjEvy9GP>k;}a@@mp%Hf?5FySdRRTR601M)xPFMIdDtwb#x(F{<^lxbF(}O2M7WWp zl2Z1I|46W47x`fC9WM8*U=}&;9?~EtEz$n{MNV}jhKm(Yw$~vO&R{W4Hb*>XipJ>;XH2Jpx|a+wMXI;lt6wo3Z)Ljs`DHXyJ)$LIq``b zD^gxc6cys%uUQ7+5cWzYV*7mU@Rfg|8&gPjCfdIbLD}~qVEcDktbY!{zmfonO8n{L7g&g|Bl-aN0_nVe5{2&8e+`xB zMjki8%CJ(Aq9@AD?tZ1GGLZ5Aq1*=~L5L@!tSX&ponNexPDz*N=h8YKH9L-P81rF9{!7(z-F7_b$_>=@tomyjdThM!y<6Bae zY{vdG=_1{p8)N}8ioS;C@(dr@R_)}T5C%c>V|b~c;5LhRi;iAu8)R}ulL@=&s@Zk6 z>}ySWoQ>vDwvcTPx>kHaVbZ+SX}@rki*GH~J4+^t9PC z=u|fHt=14)lle{6cYvOX)mZ&GBJ2{g$@KN8b~e?65RAYOh7N;tzih~EAExjN@1q+I z%{fZHMf2P&Y=78aW10S)9?~lu7_`s|<`1A++aoC^NWXxm+jurhppAHvH?dRhvT4g} zhq=&!vD%Yows`SWp3OsVWit8a_qg>5DDv6w@3>Lm9=CAtDXgJv-m&d;~GjW^oz$Nk(#o z1@_a2@uE@10q#}vxN(esT?KbwBA8PA?NrPEpYyT)cg5-dgKbER+m`sAk2Ta?uU_9) zg!RR|*tAsgGaqGH!bakI{!w92PLLRFM>=soXI*OIYUm4;7fv+@-Rlppk~yYy-;f~Y zcJ%Gk`t85CQyCv0$GhmhL<<5aHHdw~BEFM9lm%|p%#Hbwp&mQodTollzGque(8vY{ zR52gtrQ4dcCO!$xA&Ru#v!AX@CL$(HRaHtn!s|1duc@egD!o=UGEWK_r5cS7tNhs` zXU)qVDM>CVNreLwc-GFA*S^Fo;8zo42_DKC(|j8o_}K(;FZ+tK^h}zcEzqyTWWgS@ zh9q-VNo7ZrCv?L8M>F4XBPFc`LGn%7C|ap&BD@1pRflYD?8kcG=Bv?7FhDcF#Y3#* zBRajkVLtbCw0g{{;BLZUXNXE4Z14wHVE*azZ*o4JS@ma$C)d8`c`ZbJk2~_fGvavN z!>{FFkFc8!sb3(TVQQgHCSQ14xZrpu4#;GuWJm0@kuVUqKsRotYGY2ARIOEe##N}v zbX>=47@whw*!`#5H)A98{>QVNI>*K~_FtOT@KY!+UcqjB1B4c-kBRlkrvGYy$QybV zF8{s^o4$h=|CZeN&(Hsd7yXB2N>uui`3|dpKDi%`*(GRz2+1RcH;9hQ4`lzsvXF{^ zASDO;(yU6hckQ&eg3FKILw=zn1_~wR^}Q~zbJj$#j2DQXx|*2syq}!7`gpznAoJzm zJ{9JZ${c8jVh$6aDWuQe$D)R<=VV3+B8O&3?z7tEs@|;vc)&p7En(D+ufG#Db6+i2 zG_pH>tN{ti&V+3C6i?=zx8Hu>Rb89an+j^Ca#Z|_`WR}?UZ%#yU8jLIFGa^8Qht-2 zPIzqsHkga93Dl`Ym)3uh-Nbi}_SsrnFPardtK(KG0R0Alo=5;j>-W%a zv;YBaW_n*32D(HTYQ0$f1D}mzt}0b00pREwqaDs63=9t4-W0$vOrgWA$;f-Z?&gN` z#Y@8Jh((?U{Aty(@Y^H#kv>kR!#)il7cQQrqnK(M8+N!FX;TKysz_yWVeZyih+bxz zPFhwq*I9wiJQZaX@R@Fd zhm)M^g4J!ocM&Sr#Je(})eKrZfmJTtsBOj#%QhS~p?;xq0xat>K!`S6yqJ+fOHe7RiPEXH z=n0VtGLibuH)7tE89ep3(GVosQpm zp|j;a@eEz7Rpe-uw=-^hN9oU9&rT-Yo*rL_J%lQb4~8PawCJ#I-}SFFF?tvaaBG!b zTBym%9f;9t*5>+-4c`T6gEj75YQhMztT$#gMLkh}wXQgjGilvp^{t|I(d@IA0>GVn zVpcietfni2yDnL&wq|Q@girp$h%7qMbnk`ys)1-$xqmNOeHiRAOobh0h4dia@LIh{ zy#XGd*48bZ$YIF~Nt-&b2;LJ)iLy;M0aw48LMd|`3NK3}exvO%Kva$Hkbmypq|qc`#aotE2e&8Cg`toXsxK7lp#v2NQs4T)#v(*T` z4V-l$BJ&{B?HBmT8)3|K-ss)Yn$YH3|v82T4{qFo{drP++b-XdQ8sW`iIaxs@bhmv(W2Fxcau^uSMsEK>Rj z73{pi-93B=GkRE^q(gv}Me`lRD$4u##NtahUMW~WV<_G(mZgpxEkT>ktO&T}AiKv) zYPQQC9FaFTI5u-gy3R1+TJ&fCfwY)wTXYdcPDt(be=m1EX>Vna?{aVX*1{P79o+jr zI=)23ZJRl{?>rL)3bcdo`T_?kA{z$wVkc$8Dd{}$~`4ejC5hO@{QnXc#T z0QlFBFY^6Xn)J?tY@wU`ojVNF&?|( zbnfCK%xS|Q_1F^Kz7K?C~u(8lI(naxFtb;QU!&?z02`H&FF z!mkS)m6y@=PwvK@>EsMeD+WefGIOsvHuV@0?F+bwogS6kg5}ae=zx=nP;tE?I({Q9 zVRtg!inDjc7#8DG$VPEZA`5Im)BVEC9nv_2iK;;wK}ioH&CPgGbexUQ@(Sj9_!r)kvXCJ%encU1>SYu&bJCU4kM% zu&#jOS{6FHo~6ie5+zx|y)N0k&eb>APMu|luTQ!uedH$Hsv?C|)pDP8od%Zf@L%DB z?d11_^zWLo_?E2r{+*gqwzl}c2v(iS;|kx#LLQem@jm+B5D2$HA>`r^fywY7wJ~#Z zlu(rd>NV}eigu2Sg3_d8bT4$Y1!1Cz(0o0K*t*bc)*B~uYRT4w>&?@r zUBxz}*FN1|;CfKaECVr%Gk{uFjmY}Z+SHu@@koWD{1&W1mY!%e<_Q}MIwi={u_m2rB<#9V4J9>?*vl5oRZfXJTmY|e!7f;(GLTw$3dyXdC-ur& zs_ZQKr0CpVi2L-7ErFzqvnpB^fdXWKiYzKQQQ2%ZnB1O5i8%H>MR9pfj2#q3(f2sp zVrO!56^9YP@>1p*qBZ4b(z8B}iwWo#QPzJfZ2n5J5;l5WWJQI2))jQh@YnAnpn|kj!GlSHn`h1%4Pf10 z#$`L|cVl)t_`K}u(j}W>gTh}T{@E_S>wj}-5oWCtG&&=!2_|H?_mnV%zl1v9mRA+J zCMJ^31?>7-WTFszA&y6w3_lSx!8<+n4o@pN{Lvn?<(T0BQ29+UM7(g`QwA~LQZnP4 zU<-r)B?xOkj>kLd9>>fmqNQU{&&ZyHsS0l7`|r20kw*Fg+V}Ep%kOXy>A!Ju{=wRr z>gIY{gR!3yX{l`P-^*cF>v;4mcY)877@BGh6?uPPO0p)^#==jixyOm%O^2i+HnD$i ze?W{vh|)s_^3w|j@ozPP_FI*1=|dX1LRy)u(_anX@r5O@{4qT2{jrrkJ8^;;`Yz`p z>!R$W?6kPNC|ix|@r2;3ey4=Td0YGEQ?Ht>j(7H!;}2=V^6W0W$^`7 zI4ep!?~O!v5~B<=*F@yi7{w_Ts5@e*KyKL4voF&)g4EC{VF$Szr8e2F46~Y@w1hMV zB%|OUt0FB_LN@$5!IPUVer2bGG~Q`Jtd_L+EQLyuIkjw*8Ta0}ElPt!T7GJ#Kxo*& zonOLfp)?We+vTM-Y)^7ym3oj22{2xeP&!pdpt(j%`AtU70i5Ar?K>M$lchY5>M(Uj~|*+YrLz+Z9N3Kui`=?Fe|1= zh!)mB7k+gDHRK;^CKd1GKRWJjSI>*YMszDj=op$RO-x?XI{$YHU5cHrjt6NIvle|B z#L$juDFK31N_xp**g>|YiJyMW_!Wp>UXUE`c*Np>XD~WQ6<0EWeTxkBn;XiVq$xQnv48#Lm*K9f1Q8ZhUc3t@ zaByP4iMp@`I;U1fwS$bkGAwxxx!D;{Fr(r!oG;(WaktP|&V_b?=8BQmip6Luj5$0| zhc~53_*^ZlbQ-2(Y8FF)29@X0^xnMcQ5Se~#b*hLhQt+n2DLTSmsT`OMuM0oSz=k* zm^XohSF%XMksLI`ycclL8ia^bIX9+^&a4uqXvT>sPv0wq!P{{4E3DjB=sm@V$Y7%! zC+sm1RYq9hN$~{yN{e7VltX_cA)c|!n;*q?dYXczgf!fg(noPLrnnxesgD==To z8kL8^Xe6-n;aMKLfz8PlRF#MSv?4>??F%vaeY|2;u^2((FqEY{<}^6LdJYlC1ZqB3 z2{oA5)w({3mp4GtYs<#=m=-G}^`WExESws{F`1^KHG35pCaemZYTNP4S&coDVz1)h z8*Z79OCNUVzXp0;MeWe`E?DxliQF|%2gv+p-JXPDdv`g^VtVM@?JFJ?P6J_C73sK& z0ASccOU!}Lgai6b!cl)%Gh6~G=;U>AUOIwkc2>p3YGZLOhFEDwM3HA02;!~cRX5T<+xEU;Np547z(7REiT>>AxDj?=02(=YF7$%UbodGTeWgW)mhUq%ohVGsscH}xZ zFvAmi7P59!*J~lG8ifrnwf6T!fOnxnfy+8QVkBu4a81qdeDepEiW>$<4BTR0#DoQW#Xh48w zkOr5#77d`5aa;OS*H+0?*2SoI*}r^XC-_7qOqyh=csx#Lg>hkQ;q_?!}lL-SJD0?H4&BRTO`(T7`&1=fH z0g9@7?8b;wGwu11oSm{o@(2a)+v}dEcFaqdFJr`Tp%QNrqmIDFSa17nefwd?;NaEU z(#gt`FJTu}HP<`XFin|1%8^^}AmpUB1EQQ$c0SzBm)=_Eg<(8417DwupI)rljtaNr zZ!AN8cyEV!L^3VFlg#OVE8?Kq_gdBKK8{@L9YI6kM5O`k4C2vLnrurQ>zRO>*pd){ zz3B0|ccsUkB^<*IiL?N3Kcj2iHMHJbD41!e)8V1H5xSTc=e~^O90+yHjLh1Wa+A!h zsoiZ6;mE2e)6``%fiuL#d5-M={fwoxF9fU!#-A*n=IWKM&w6fl-e<0p zdsn$Tzxt~Hkl3`0vvVNwF?#PRg}gj1OfgXZX(wfV=*t!t0bR$4n!F}W{m&0LlNF>A&2Jm-taK&Yln0GU5z zg!R9P+|Jc4c&$~?;e0^r=y@EmV%*K6r^IyM+Jo+v?U}Zaph@_=ol40*wb0{(PeHbw z>xTsnVu8b9`43^L!`Rw3ZM>{%%-%P=J3nCihI4UopHu_=f*oEV;eU>t>SB?$kzDv;~WH^`S`elYG z*-6@0jA_omI-bj}^^@vts~0>)LPgL8s+ErVUw*UB zn`>FfTXiWa>Yw|TgrdG!mqU0}+vBytAJ2b>*|<^jXExZ(40s1!Ut^ay;5%C{%nu$2 zbZvhO{fsa>86G*RgW~X&k394u-+}H!zIo7Z&};6f5()C}?n}|IG45FpuWdi9^=+;x zLEm@I&%xhMM?DW5^0LP-2JU1xXOkf`?vdP!_h6`9Lce+3LqXD#@fSzqSMJfQsX>po z@MJYcqzFT;M4JJ6KWrV@<4Ke*#febLn_ z>w@cZkC(cLHm<6wz6*Xncuo@WbSZYya>K>a#F$Q|dc{UKB&?WBzW0e+N)Jg&82PLQ zj>?XA{Sm?dxM?5gAqP{{fM{M1+0cp!ZwQS$68d&|B}{jputRd}xdt{nA9Q$@l1OjN zwPBRPEZM+OjDqt}$}*WW&=}cSj4W?1h_)37eOx+ZRA=B&{?i+b>yYDNWV}UbYk=)Q zP>aH+hvg2lDxPoOodbaFV4spi`Gh}cc6QhgZ_BsdPLKH=`oZCekYCCWnS}93Y+G@} za!L0GzeR8iHDvG>isJs$IH~dIu+43%6sAgXN?`AKa`S4wTD&sOfq!yL+ooa`CK*a5zP0v<5_Vz--GC62C>eyW3Jv6(Yq3-K%NWL6Xy!!|CEm|)Mz%W>E z8o}p}6cv@1RSD1*Et%D)=A1BlM=CzT0YvvVP&fOXK}KZ{D8k`P?nVeeRZiT)*pEM% z=FU_qeKs+p%;7KvQdJQe#e{H?@5!Jesxq)<)e46sH(6w?SKJ)^FkwkxQ^6~{Jy>!L z?-0%cPaPB9Qg7@EGm^=Q4d9)a>IGPIM!an+Kj=s0)XsqsL{vM{mxvH33e!z(xV#6{ z`Ke{~DFS`$k{wC!l};Mz_P4M{A9wg2cg30(J!DExlI6~DOy0jNOTs*m^C+sdVS>|8 zKQbY|-cZxXWaaYAPh&a(6n8nMC$E#4Ax1dG1^7U`kbyP)eNt<$z# zeKqf8_zvmg@OpT5%}K7@-KjUNJ3r7^Rf>FD;loeDy{U_?lNQ`5X zXHyC%i3!D^8iGWLS`tcKhJXqJ60@d+&adg%I-N)y%VpG8B@euw1mA7gj8|K2kPH>G~2^m))x1XKx$48W}sSyxP{S^wVRF|HV zSk#xKrLp;$DhJ9vDqaY%EILEM2Ie>ubBPA(l^rv|ENJbGe@9V+j@`0`*N(IrXNb+t z205{qs|n4g|1uYbn6-A<23RGq1$3V8EW-~7xP9?syH(BlAPhezomNa`j4br9Fz z)=~FT)xlItaCuX3-KK2-mJdlf2&(s_-7;NWiW66eC_FeWNyhAkMMLJM8Npo?+Ozl3 zBevk_Vd?ByzGrXwCsVhv6s(Tp+}Ppw3y4LwYlS3-2BbkP8R^(QNOla#O~s?%vbkoe zBg7QnQr#UJByEJVsd2iM+}^v!s~Q^P|b?a;Rxpn}(?tsFwEWKETpFp4?3BvCi5gy4)HQYE#UD<7N|{(C=aHd(2(eQrshhDxlelF8qM>` z?!0>eag8!)0GMz9P1*xxHa$t6>2EWBNqBCD`#9Y24Ad)Tu`6xK*_p{(M;4Dbj0LQy z%O9jFpEv&AJWr7I^R~32?HCc~v6<%wf!D(hX9T6A8GT&3cqG%Ov}t_I^NJRnkCk?) z40aie{3tP3S-krhh($@gBH7JJs$BGY!0`02RLo%7Lxm;5!mS%1%yUC9v`4f>ieE4H z#l!OqX^|s43*g(cuhNd>V;JW(jq>3?_#5Zu!R`cQIIF)&sZ$kIb0@Y*8LZGeMsTds znrK>jN8=W3HoVhJ8%0!N;w!@&QL5YHfg-HJ%tTy__Huju0)K2$Wl{|%)5`w*z1p=m zqk(I6-12zJ=u`GR8QMYSslPAtZ@0EflK#cS$XoUTvUzAD5C{~PM{Op$pD8|ftE~PX z{g+?P+@KCOnx(#?cP%8e!)k;X?=ysdA>^SgL=k26OVx%=wa~L|(d(mYv!{8dcze6j z_h|LI<1^Y z5rl?QRzUbq<^7^<3Nrw4iZW@%LvB%uj&Gr+rJ~GIy%hkFrYABRAUnS$q%D0>;?e0F z*YC*NTZCx#;`B%J6dANYbnJuKuiyJ@rPo1!W(yoV9-N|E*bi?ZPSQpCp{sJ6NZ*CU zkKUycUA-@@e-CT-x2UC~bWalsYqBGg!6ArFWmEw1t)0(NT zZ%ah9P*p#+ogxb4pG<{n=s1{w6yf)5Pnc7k->i4J$D=#oy!(LeDbH6emaBR=LFm?bmTzLCYIaUSX9i+(Np3Ech~* zZHTPZ`qMW7@!C0m)ySk|8>=iz9uk3a={c)1BmX_(iy>YbGwBzbB70ITRD;4)n5Re3 zv3feudeh@Wv$Z^3LRkfij>W8`O&Xe0GmItv={wtBH*eWd&MAov7wPat zRX+eoZInHV$FwzpEE#?ASl&^}UDi!0=un=cDFEG_WE^xJtRnhKeVAkBcPLe5t$F(B zdMxkAZQBM_DexyTjp?KgPItFnTep?d7nJi;%7+2_B3wz#V@$6<-6N=m@0Eb_ma<*2 ztl1m5s--y1ew_AvXWGOBMlS{P^oSw+WJ3-`l?LTUxly?Y@u^I6d#dM}QeckO61;u5 z*oLSY({aV(R;c;E4J-16B^vd3ZXp@#!TXInjaahq0>{!8;$%ZPqW!!dTfeZcQFyZ1 z>`NnKReAcFyh{VoCo(Ecg&r#L7$AT&J50!dWuZCSI$7O;2*rs6tQS_bbKP5x$#Btj|uuR!tp8n*%I3T z#I*o#zgxZ75dLNmV{k-117H-Xi89zDKYCfrph%G{*9i8aW)#fi>{Od&bOn&EF~ftt z+7Pq>z)@g8x%{iNrNriHjL8#Tcz|$oqk6D3K2kKbzn0Hlx!8MjN0IXyEo3x@M3g3*q)7 zf=$>mM3McVz#U|myVoDXx{f+xFGNmwCa95_dZ&z|Bvtyn?%{DPH&dD&SoE3s&_z0x z;~M43AnS-z%h+87s-#;(dqrM5{(uxI-x``q{p*WxUWkEWpcdlud)Nt*NWi7ZdDIrC z_*E;|%V30~wZFY1*p<%OpJEBchiO-F5;>!XwzZz1kddp zLZ#w8zx>=scB@Ztd0c#j?z|9PpBNz*-EK)g4%Ib=AD#i#u%c_fz|}vELP1yJH;%_G zBIz&kcdB@=G(LXklqV+FuusvJHyD%Dgh&vGat^kil{edhO2WkgZP$cFd57ALEfGEm zA{ooH`(!1zw_6z}?LjLUIq8nv7yXTl)rjW5#`YLa&C~01FLasqF-bD~i?@MUFJQU& zSK^=jJ}|QE;-6WsfAZ7xKB+J(n3l$B6d_yYh*tf=XlZKuwE1eZmsuk&H(f!fH*$*- z=8VRBrHYD*9hKoEhI<&FNX$4HtbcL+-fc8Vrj^C=axFkI+|CN6am>_(t&OL%n-LR| zXL0(#i=SzkCh-Z&b)93uyM`NMyhTR&m(~3<4n_DN8BWx=fa0lu|1Wo@HZ_;#WnRA` zFqhUtg=`xdz#g5)lATxmS6KhH?*TGIn9kY;$7BRg7*A5X&9B*MBPkOrMH%aA`I`Ybng+8#5_=~W4X{{&s zp|@|-*oP4uBv0IA7toH!!d(J7dy@Ny_DjwVaC~P;D|)N5{HHp?{K9H-kn(a+Nk${B z{~CaG+Xi)9`xa=0zdbJ0|5IlAA7J1gd)GgZAo4rry6_u?XS4cB)X(^@9Ed(@ps{>e z$;(f|5Hm3q2K9j6W_=e0u=dNMOQhZ68_T_L_>>Y5@dZ<#gj*R+J$2&S-1*dXk7=Ic zjqk;++de;1`r?`E$jeg1i2Mzpa9gs94gq1K#1G6!EvdaUQY3boUDqWoRNM3Rt;Ks? z|EIDufroPId>lu~1>khSb`Z}t=!`zW%eR6~<(n0XDNNTWf@b}bdxZX%T;np@o~ z(jpSKP@+_Hy(&v?mP+^bo{8~rj4|)&GoP_^zP~ePd(Lw_=l4G;fL^t`kw|tiVN}*L z&USsIm7Jk{c%)>R9*x(!@`lVOub%65yrN#sRP#t;S$u}Rid7@pCX|9Mh#q$0D>wVy z`ks^`e)vp6hryw}6~U=;H&Wd3y($#i=Gfb3f0I37m4Co6CP43!Z(x-N`X5osp1tms ze%c3}6kDxdVi;xvDg5Kk=TLkvqlYWfL@LvboWsVW+U`h~6rz383{`x@j1I34O>A9u z(OF!w(7xw%ab7W5$HpM}K%Mf9$YGm+jk=D;r>mTjH9CcgYjXwbLtab1OI>AUy5g{C zP+qH{X$!n|DOCvC7Z1h zLb#ijLmCEVemlBALG`lx+>j-CJM z{h@xv#Js&KqkRhBOy1ko*g1^9E1Qrp(!v^?%anZ^SMoN$#p>Wa#eciXlWFTD1ES($ zH&V4-ltR*P33%k}#G;=mJh;o#As5=>+aU21_EK|k|9@jb19hYPwg}ym-xdxYfL#h6fHhzqHN zYkcGRSE)zjf>t}WM{V$3mj0`ekRsBM<`vXf`EFyewPD2G@^lO3*a69qCC@P{(GljB zE`En-IER~AWiM9AR!j4{Uk=#yOt;C+#-Op<(;EA!y|FJxLO9WFXBeaS><3EcaP&*( zzo~{Dmbt3xpYxQDABzsC^mB-j_Y4fixsHDJ@(yo#wk?L1;9ELcW8OHntM9o~DYh@8 zuPLcd@fq&(3&k|dQ~tzN!->&}k}9$L;?Dn7wRQCA2?Hg$*v-@qnn$E{Tf&&2xYXs+ z_LD(>AN;Ua#b*3^n-u!hwIU%`r>>7{oU5eb3t#wbl-7!T;3rgjJ92pfS?_rEApy7Y zS9*>cy#}|gS#39hFKYTV!#^#)X~5`sPNONB&!GZCky=_LR?Jg)3KK5)P-{=pn-RD7 z|KV4UFm2h_XU&_LWA-qv&zCnd!%S81{Fg%;N=8@A{_{GzSaQPzz=BLBF>Q^P|%BeNnwjwq79i}r|@D4J&`6WOqN zeY4?>G@M^Cmc%VrU_17)(9zUH(3Np8iJwT-!F6ng7(=exsw5C*3 z$^`UBU)w+AjcY3CzPctu1(Qyh&@|3*@)ERG>GdpMP7qb49B)w7x`l3AJg7h}x;0XH zOs6_OLo-O7?~z)8VTm_**C=p9U)bW;@Ae%!8vjrG)&fz`lo;@0df-oa--Bn=Is4xK z#g*H=;%p+BqtiVPugD@`558mx$YcUuh-p4BSDQ-0sDU59vNdxwQMcM|u4!j8JDY#` z79(TupPA21fk;WyiB1KNgrKIg*_v#(GB2B@A%#i?(d?zypHcFT)lO%(98W6yOD8?n5M)czS{wx5WqGz2>X%9Wh`BayD&NpQEt}Go42UWTnwA<_|%>>Wwvn$^e4>v zR$*TaG$)R%LWU<(G(D&=EHM@W|V)P*a|Qn z4hw+b3E`aZ&|L|Ph28KG?7aw1*qPfsFcbDhMwm-!oR~lMl;&Nk!8XJQb&MP8{HDZk z@nIuXL@4_N7sa1zs|pLiwv~uL@+mF^IG9+%O0bI^qVyq&3ni{R?O;vVhz!xpO5sA2 zlPwu61)H)UQWF_mNO7=eft6tY3qjn5ACL*xp{QoJiP>sQd;1H>C zumXmzaWkg(sYz|Yx`GcxA$*%sF8G{}N5KsPpCLiSqRSQ*W8W6=(*p?eRqY(+kLsBF zECF0j_>T|>v%g_sCZ}r@ymgC^g`4J*x!=fzKLNa*i0Hg+o}&Y=W@mJx1uo<878fG( z+vDkl-FzEfaG9BzS*t|m?iMT2se)iLW5(_odEUJ)I~zW5%Y{PefPe47&D?g75rz66 D613UA literal 0 HcmV?d00001 diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties new file mode 100755 index 0000000..2e6e589 --- /dev/null +++ b/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=wrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-7.3.3-bin.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=wrapper/dists diff --git a/gradlew b/gradlew new file mode 100755 index 0000000..286621e --- /dev/null +++ b/gradlew @@ -0,0 +1,183 @@ +#!/usr/bin/env sh + +# +# Copyright 2015 the original author or authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + +############################################################################## +## +## Gradle start up script for UN*X +## +############################################################################## + +# Attempt to set APP_HOME +# Resolve links: $0 may be a link +PRG="$0" +# Need this for relative symlinks. +while [ -h "$PRG" ] ; do + ls=`ls -ld "$PRG"` + link=`expr "$ls" : '.*-> \(.*\)$'` + if expr "$link" : '/.*' > /dev/null; then + PRG="$link" + else + PRG=`dirname "$PRG"`"/$link" + fi +done +SAVED="`pwd`" +cd "`dirname \"$PRG\"`/" >/dev/null +APP_HOME="`pwd -P`" +cd "$SAVED" >/dev/null + +APP_NAME="Gradle" +APP_BASE_NAME=`basename "$0"` + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD="maximum" + +warn () { + echo "$*" +} + +die () { + echo + echo "$*" + echo + exit 1 +} + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "`uname`" in + CYGWIN* ) + cygwin=true + ;; + Darwin* ) + darwin=true + ;; + MINGW* ) + msys=true + ;; + NONSTOP* ) + nonstop=true + ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD="$JAVA_HOME/jre/sh/java" + else + JAVACMD="$JAVA_HOME/bin/java" + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD="java" + java --version >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." +fi + +# Increase the maximum file descriptors if we can. +if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then + MAX_FD_LIMIT=`ulimit -H -n` + if [ $? -eq 0 ] ; then + if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then + MAX_FD="$MAX_FD_LIMIT" + fi + ulimit -n $MAX_FD + if [ $? -ne 0 ] ; then + warn "Could not set maximum file descriptor limit: $MAX_FD" + fi + else + warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" + fi +fi + +# For Darwin, add options to specify how the application appears in the dock +if $darwin; then + GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" +fi + +# For Cygwin or MSYS, switch paths to Windows format before running java +if [ "$cygwin" = "true" -o "$msys" = "true" ] ; then + APP_HOME=`cygpath --path --mixed "$APP_HOME"` + CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` + JAVACMD=`cygpath --unix "$JAVACMD"` + + # We build the pattern for arguments to be converted via cygpath + ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` + SEP="" + for dir in $ROOTDIRSRAW ; do + ROOTDIRS="$ROOTDIRS$SEP$dir" + SEP="|" + done + OURCYGPATTERN="(^($ROOTDIRS))" + # Add a user-defined pattern to the cygpath arguments + if [ "$GRADLE_CYGPATTERN" != "" ] ; then + OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" + fi + # Now convert the arguments - kludge to limit ourselves to /bin/sh + i=0 + for arg in "$@" ; do + CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` + CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option + + if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition + eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` + else + eval `echo args$i`="\"$arg\"" + fi + i=`expr $i + 1` + done + case $i in + 0) set -- ;; + 1) set -- "$args0" ;; + 2) set -- "$args0" "$args1" ;; + 3) set -- "$args0" "$args1" "$args2" ;; + 4) set -- "$args0" "$args1" "$args2" "$args3" ;; + 5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; + 6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; + 7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; + 8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; + 9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; + esac +fi + +# Escape application args +save () { + for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done + echo " " +} +APP_ARGS=`save "$@"` + +# Collect all arguments for the java command, following the shell quoting and substitution rules +eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS" + +exec "$JAVACMD" "$@" diff --git a/gradlew.bat b/gradlew.bat new file mode 100755 index 0000000..24467a1 --- /dev/null +++ b/gradlew.bat @@ -0,0 +1,100 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem + +@if "%DEBUG%" == "" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%" == "" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if "%ERRORLEVEL%" == "0" goto init + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto init + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:init +@rem Get command-line arguments, handling Windows variants + +if not "%OS%" == "Windows_NT" goto win9xME_args + +:win9xME_args +@rem Slurp the command line arguments. +set CMD_LINE_ARGS= +set _SKIP=2 + +:win9xME_args_slurp +if "x%~1" == "x" goto execute + +set CMD_LINE_ARGS=%* + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% + +:end +@rem End local scope for the variables with windows NT shell +if "%ERRORLEVEL%"=="0" goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 +exit /b 1 + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/header.txt b/header.txt new file mode 100755 index 0000000..ff1e0c9 --- /dev/null +++ b/header.txt @@ -0,0 +1,16 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ diff --git a/kit/sds-mk2/build.gradle b/kit/sds-mk2/build.gradle new file mode 100755 index 0000000..856270c --- /dev/null +++ b/kit/sds-mk2/build.gradle @@ -0,0 +1,24 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +/* + * Swerve Drive Specialties' MK2 Swerve Module Kit + * + * This kit uses the RevRobotics NEO + */ +dependencies { + api project(":vendor:${rootProject.name}-RevRobotics") +} \ No newline at end of file diff --git a/kit/sds-mk2/src/javadoc/overview.html b/kit/sds-mk2/src/javadoc/overview.html new file mode 100755 index 0000000..66c459a --- /dev/null +++ b/kit/sds-mk2/src/javadoc/overview.html @@ -0,0 +1,12 @@ + + + + +

This API adds official SwerveIO support for the Swerve Drive Specialties MK2 Swerve + Module kit. You should have been directed here from the official SwerveIO documentation. If you weren't, + please go there now for instructions on getting this API set up for use. +

+ + + \ No newline at end of file diff --git a/kit/sds-mk2/src/main/java/net/bancino/robotics/swerveio/module/MK2SwerveModule.java b/kit/sds-mk2/src/main/java/net/bancino/robotics/swerveio/module/MK2SwerveModule.java new file mode 100755 index 0000000..7ed318f --- /dev/null +++ b/kit/sds-mk2/src/main/java/net/bancino/robotics/swerveio/module/MK2SwerveModule.java @@ -0,0 +1,121 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.module; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import net.bancino.log.Log; +import net.bancino.log.Loggable; +import net.bancino.robotics.swerveio.encoder.AnalogEncoder; +import net.bancino.robotics.swerveio.encoder.Encoder; +import net.bancino.robotics.swerveio.encoder.SparkMaxEncoder; + +import net.bancino.robotics.swerveio.geometry.Length; +import net.bancino.robotics.swerveio.geometry.Length.Unit; + +/** + * A swerve module implementation that uses RevRobotics Neo motors and Spark Max + * motor controllers. This was designed for Swerve Drive Specialties' MK2 + * Module. + * + *

+ * To use this module implementation, you must have the following hardware + * setup: + *

+ *
    + *
  • 2 RevRobotics' NEO brushless motors
    • + *
  • 8.33:1 gear ratio drive gearbox
    • + *
  • 4-inch wheels
    • + *
  • A 1:1 (analog) encoder
    • + *
+ * + *

+ * This implementation requires a 1:1 encoder. You must attach an + * encoder to the dedicated location on the MK2 kit in order to use this module + * implementation. The Neo internal encoders are absolute garbage. + *

+ * + * @author Jordan Bancino + * @version 5.0.1 + * @since 1.0.0 + */ +@Loggable +public class MK2SwerveModule extends GenericSwerveModule { + + private final CANSparkMax pivotMotor; + + /** + * The swerve module is constructed to allow the pivot motor to coast, this + * allows for adjustments, but as soon as the module is driven, it switches to + * brake mode to prevent outside modifications. + */ + @Log + private boolean setPivotIdleMode = false; + + /** + * Create a new MK2 swerve module composed of Neo brushless motors that use the + * Spark Max motor controllers. + * + * @param driveCanId The CAN ID of the drive motor controller for this module. + * @param pivotCanId The CAN ID of the pivot motor controller for this module. + * @param pivotEncoder The encoder to use as the pivot encoder. + */ + public MK2SwerveModule(int driveCanId, int pivotCanId, Encoder pivotEncoder) { + super(new GenericSwerveModule.Builder() + .setDriveMotor(new CANSparkMax(driveCanId, MotorType.kBrushless), + (GenericSwerveModule.Builder builder, CANSparkMax motor) -> { + motor.setIdleMode(IdleMode.kBrake); + builder.setDriveEncoder(new SparkMaxEncoder(motor)); + }).setPivotMotor(new CANSparkMax(pivotCanId, MotorType.kBrushless), + (GenericSwerveModule.Builder builder, CANSparkMax motor) -> { + motor.setIdleMode(IdleMode.kCoast); // This is set to brake before we drive + builder.setPivotEncoder(pivotEncoder); + }) + // Constants for the MK2 kit and Neo + .setDriveGearRatio(8.33).setDriveMaxRPM(5680).setWheelDiameter(new Length(4, Unit.INCHES))); + + pivotMotor = (CANSparkMax) getPivotMotor(); + } + + /** + * Create a new MK2 swerve module composed of Neo brushless motors that use the + * Spark Max motor controllers. + * + * @param driveCanId The CAN ID of the drive motor for this module. + * @param pivotCanId The CAN ID of the pivot motor for this module. + * @param analogEncoderPort The port on the roboRIO that the encoder to use as + * the pivot encoder is on. + */ + public MK2SwerveModule(int driveCanId, int pivotCanId, int analogEncoderPort) { + this(driveCanId, pivotCanId, new AnalogEncoder(analogEncoderPort)); + } + + @Override + public void setAngle(double angle) { + /* + * Enable brake mode so that the module doesn't move due to external force after + * starting. + */ + if (!setPivotIdleMode) { + pivotMotor.setIdleMode(IdleMode.kBrake); + setPivotIdleMode = true; + } + super.setAngle(angle); + } +} diff --git a/kit/sds-mk3/build.gradle b/kit/sds-mk3/build.gradle new file mode 100755 index 0000000..4f32feb --- /dev/null +++ b/kit/sds-mk3/build.gradle @@ -0,0 +1,24 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +/* + * Swerve Drive Specialties' MK3 Swerve Module Kit + * + * This kit uses the CTRE Falcon 500 + */ +dependencies { + api project(":vendor:${rootProject.name}-CTRE") +} \ No newline at end of file diff --git a/kit/sds-mk3/src/javadoc/overview.html b/kit/sds-mk3/src/javadoc/overview.html new file mode 100755 index 0000000..08ae1d6 --- /dev/null +++ b/kit/sds-mk3/src/javadoc/overview.html @@ -0,0 +1,12 @@ + + + + +

This API adds official SwerveIO support for the Swerve Drive Specialties MK3 Swerve + Module kit. You should have been directed here from the official SwerveIO documentation. If you weren't, + please go there now for instructions on getting this API set up for use. +

+ + + \ No newline at end of file diff --git a/kit/sds-mk3/src/main/java/net/bancino/robotics/swerveio/module/MK3SwerveModule.java b/kit/sds-mk3/src/main/java/net/bancino/robotics/swerveio/module/MK3SwerveModule.java new file mode 100755 index 0000000..5bfaa9f --- /dev/null +++ b/kit/sds-mk3/src/main/java/net/bancino/robotics/swerveio/module/MK3SwerveModule.java @@ -0,0 +1,214 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.module; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.SensorInitializationStrategy; + +import net.bancino.robotics.swerveio.pid.PIDController; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +//import com.ctre.phoenix.motorcontrol.ControlMode; +//import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; +import com.ctre.phoenix.motorcontrol.NeutralMode; + +import net.bancino.robotics.swerveio.encoder.Encoder; +import net.bancino.robotics.swerveio.encoder.PhoenixCANCoder; +import net.bancino.robotics.swerveio.encoder.PhoenixEncoder; +import net.bancino.robotics.swerveio.geometry.Length; +import net.bancino.robotics.swerveio.geometry.Length.Unit; + +/** + * A swerve module implementation that used CTRE Falcon 500 motors and TalonFX + * motor controllers. This implementation was designed for Swerve Drive + * Specialties' MK3 Module, and provides support for the internal Falcon 500 + * encoder as well as the external 1:1 encoder recommended by Swerve Drive + * Specialties. + * + *

+ * To use this module implementation, you must have the following hardware + * setup: + *

+ *
    + *
  • 2 CTRE Falcon 500 brushless motors
    • + *
  • 8.16:1 gear ratio gearbox
    • + *
  • 4-inch wheels
    • + *
+ * + *

+ * Optional hardware: + *

+ * + *
    + *
  • CTRE CANCoder or other external encoder
    • + *
+ * + * @author Jordan Bancino + * @version 6.0.2 + * @since 4.2.0 + */ +public class MK3SwerveModule extends GenericSwerveModule { + + private final WPI_TalonFX pivotMotor; + private final WPI_TalonFX driveMotor; + private boolean setPivotIdleMode = false; + + /** + * Create a new MK3 swerve module. This constructor assumes a profile slot of + * zero and that you're using the CTRE CANCoder. + * + * @param driveCanId The CAN ID of the drive motor controller for this + * module. + * @param pivotCanId The CAN ID of the pivot motor controller for this + * module. + * @param pivotEncoderCanId The CAN ID of the pivot motor encoder (CANCoder). + */ + public MK3SwerveModule(int driveCanId, int pivotCanId, int pivotEncoderCanId) { + this(driveCanId, pivotCanId, new PhoenixCANCoder(pivotEncoderCanId)); + } + + /** + * Create a new MK3 swerve module. This constructor assumes a profile slot of + * zero and the encoder is 1:1. + * + * @param driveCanId The CAN ID of the drive motor controller for this module. + * @param pivotCanId The CAN ID of the pivot motor controller for this module. + * @param pivotEncoder The encoder that monitors the pivot motor at a ratio of + * 1:1. + */ + public MK3SwerveModule(int driveCanId, int pivotCanId, Encoder pivotEncoder) { + this(driveCanId, pivotCanId, pivotEncoder, 1); + } + + /** + * Create a new MK3 swerve module. This constructor assumes a profile slot of + * zero and uses the internal encoder. + * + * @param driveCanId The CAN ID of the drive motor controller for this module. + * @param pivotCanId The CAN ID of the pivot motor controller for this module. + */ + public MK3SwerveModule(int driveCanId, int pivotCanId) { + this(driveCanId, pivotCanId, null, 6); + } + + /** + * Create a new MK3 swerve module. This constructor assumes a profile slot of + * zero. + * + * @param driveCanId The CAN ID of the drive motor controller for this + * module. + * @param pivotCanId The CAN ID of the pivot motor controller for this + * module. + * @param pivotEncoder The encoder that monitors the pivot motor. + * @param pivotGearRatio The gear ratio between the pivot motor and the pivot + * encoder. + */ + public MK3SwerveModule(int driveCanId, int pivotCanId, Encoder pivotEncoder, double pivotGearRatio) { + this(driveCanId, pivotCanId, 0, pivotEncoder, pivotGearRatio); + } + + /** + * Create a new MK3 swerve module composed of Falcon 500 motors that use the + * TalonFX motor controllers. + * + * @param driveCanId The CAN ID of the drive motor controller for this + * module. + * @param pivotCanId The CAN ID of the pivot motor controller for this + * module. + * @param pidSlot CTRE motor controllers support multiple PID controller + * slots. Specify the slot you want to use for both the + * drive and pivot encoders, as well as the drive and + * pivot PID controllers. This value is used in this + * module implementation wherever Phoenix requires a slot + * number, so make sure all your motors and encoders use + * the same slot number. + * @param pivotEncoder The encoder that monitors the pivot motor. + * @param pivotGearRatio The gear ratio between the pivot motor and the pivot + * encoder. + */ + public MK3SwerveModule(int driveCanId, int pivotCanId, int pidSlot, Encoder pivotEncoder, double pivotGearRatio) { + super(new GenericSwerveModule.Builder().setDriveMotor(new WPI_TalonFX(driveCanId), + (GenericSwerveModule.Builder builder, WPI_TalonFX motor) -> { + motor.configFactoryDefault(); + builder.setDriveEncoder(new PhoenixEncoder(motor, FeedbackDevice.IntegratedSensor, pidSlot)); + motor.setNeutralMode(NeutralMode.Brake); + }).setPivotMotor(new WPI_TalonFX(pivotCanId), + (GenericSwerveModule.Builder builder, WPI_TalonFX motor) -> { + motor.configIntegratedSensorInitializationStrategy( + SensorInitializationStrategy.BootToAbsolutePosition); + motor.setNeutralMode(NeutralMode.Coast); // This is set to brake before we drive + if (pivotEncoder != null) { + builder.setPivotEncoder(pivotEncoder); + } else { + builder.setPivotEncoder( + new PhoenixEncoder(motor, FeedbackDevice.IntegratedSensor, pidSlot)); + } + motor.setInverted(true); + }) + .setDriveGearRatio(8.16).setPivotGearRatio(pivotGearRatio) + /* 6380 RPM x 75% = 4785 */ + .setDriveMaxRPM(4785).setWheelDiameter(new Length(3.875, Unit.INCHES))); + + pivotMotor = (WPI_TalonFX) getPivotMotor(); + driveMotor = (WPI_TalonFX) getDriveMotor(); + + /* Sensible defaults for the Falcon 500 using the integrated encoder. */ + PIDController drivePIDController = getDrivePIDController(); + drivePIDController.setP(0.00008); + drivePIDController.setI(0.000007); + drivePIDController.setD(0.0); + drivePIDController.setF(0.00002); + drivePIDController.setOutputLimits(1); + } + + @Override + public void setAngle(double angle) { + /* + * Enable brake mode so that the module doesn't move due to external force after + * starting. + */ + if (!setPivotIdleMode) { + pivotMotor.setNeutralMode(NeutralMode.Brake); + setPivotIdleMode = true; + } + super.setAngle(angle); + } + + @Override + public void setSpeed(double speed) { + /* + * The CTRE motor controller expects a velocity in encoder counts per 100ms. + * This converts RPMS to counts per 100ms based on the number of encoder counts + * there are in a revolution of the motor shaft, then scales it on the percent + * output reference we're given. + * + * In hindsight, we should have converted the sensor velocity into RPMs so we + * could create a unified velocity loop API, but once upon a time we tried to + * use the motor controller's PID loop, hence this conversion. + */ + double setpoint = speed * (getDriveMaxRPM() * getDriveEncoder().countsPerRevolution()) / 600.0; + double feedback = driveMotor.getSelectedSensorVelocity(); + double output = getDrivePIDController().getOutput(feedback, setpoint); + + driveMotor.set(output); + } + + @Override + public double getOutputThreshhold() { + return 0.02; + } +} diff --git a/misc/virt/build.gradle b/misc/virt/build.gradle new file mode 100755 index 0000000..ff1e0c9 --- /dev/null +++ b/misc/virt/build.gradle @@ -0,0 +1,16 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ diff --git a/misc/virt/src/javadoc/overview.html b/misc/virt/src/javadoc/overview.html new file mode 100755 index 0000000..e568126 --- /dev/null +++ b/misc/virt/src/javadoc/overview.html @@ -0,0 +1,13 @@ + + + + +

+ This is a virtual swerve drive plugin for SwerveIO. It is designed to be used for unit tests and simulations. + The classes provided by this library are intended to be as minimal as possible and they rely heavily on the + SwerveIO library. This is so that all the SwerveIO code can still run during unit tests and simulations, to + achieve the most realistic effect. +

+ + + \ No newline at end of file diff --git a/misc/virt/src/main/java/net/bancino/robotics/swerveio/VirtualMotor.java b/misc/virt/src/main/java/net/bancino/robotics/swerveio/VirtualMotor.java new file mode 100755 index 0000000..ba84779 --- /dev/null +++ b/misc/virt/src/main/java/net/bancino/robotics/swerveio/VirtualMotor.java @@ -0,0 +1,71 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio; + +import edu.wpi.first.wpilibj.motorcontrol.MotorController; +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; + +/** + * A virtual motor that implements the WPILib {@code MotorController} interface. + * This does not map to any hardware so it can be used for testing. Note, + * however, that this will not change any encoders or gyro angles. + * + * @author Jordan Bancino + * @version 6.1.0 + * @since 6.1.0 + */ +@Loggable +public class VirtualMotor implements MotorController { + + @Log(as = "rawValue", atLevel = LogLevel.IMPORTANT) + private double speed; + + @Log(atLevel = LogLevel.IMPORTANT) + private boolean inverted; + + @Override + public void disable() { + stopMotor(); + } + + @Override + public double get() { + return speed; + } + + @Override + public boolean getInverted() { + return inverted; + } + + @Override + public void set(double speed) { + this.speed = speed; + } + + @Override + public void setInverted(boolean isInverted) { + this.inverted = isInverted; + } + + @Override + public void stopMotor() { + set(0); + } +} diff --git a/misc/virt/src/main/java/net/bancino/robotics/swerveio/VirtualSwerveDrive.java b/misc/virt/src/main/java/net/bancino/robotics/swerveio/VirtualSwerveDrive.java new file mode 100755 index 0000000..4b079ff --- /dev/null +++ b/misc/virt/src/main/java/net/bancino/robotics/swerveio/VirtualSwerveDrive.java @@ -0,0 +1,56 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio; + +import net.bancino.robotics.swerveio.module.SwerveModule; + +import java.util.Map; + +import net.bancino.robotics.swerveio.geometry.ChassisDimension; +import net.bancino.robotics.swerveio.geometry.Length; +import net.bancino.robotics.swerveio.geometry.Length.Unit; + +import net.bancino.robotics.swerveio.module.VirtualSwerveModule; + +/** + * Static methods dealing with a virtual swerve drive (see {@link #create()}). + * + * @author Jordan Bancino + * @version 6.1.0 + * @since 6.1.0 + */ +public class VirtualSwerveDrive { + + /** + * Create a virtual swerve drive that utilizes all SwerveIO code, but doesn't + * map to any hardware. This is used primarily for testing SwerveIO code. It + * directs all outputs to an entirely virtual swerve module, which is backed by + * virtual motors and encoders that are updated using arbitrary characteristics. + * + * @return A swerve drive that does not map to any hardware. It can be used in + * unit tests. + */ + public static SwerveDrive create() { + return new SwerveDrive.Builder().useDefaultKinematics(new ChassisDimension(new Length(25, Unit.INCHES))) + .setModuleMap((Map map) -> { + map.put(SwerveModule.Location.FRONT_RIGHT, new VirtualSwerveModule()); + map.put(SwerveModule.Location.FRONT_LEFT, new VirtualSwerveModule()); + map.put(SwerveModule.Location.REAR_LEFT, new VirtualSwerveModule()); + map.put(SwerveModule.Location.REAR_RIGHT, new VirtualSwerveModule()); + }).build(); + } +} diff --git a/misc/virt/src/main/java/net/bancino/robotics/swerveio/encoder/VirtualEncoder.java b/misc/virt/src/main/java/net/bancino/robotics/swerveio/encoder/VirtualEncoder.java new file mode 100755 index 0000000..408e97f --- /dev/null +++ b/misc/virt/src/main/java/net/bancino/robotics/swerveio/encoder/VirtualEncoder.java @@ -0,0 +1,62 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.encoder; + +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; + +/** + * A virtual encoder that implements the SwerveIO encoder interface. This does + * not map to any hardware so it can be used for testing. Note, however, that + * this must be manually updated by some method using {@code set()} because + * otherwise, the reading will not change. + * + *

+ * This emulates a quadrature encoder, so there are 4096 counts per revolution + * as reflected by {@link #countsPerRevolution()}. + *

+ * + * @author Jordan Bancino + * @version 6.1.0 + * @since 6.1.0 + */ +@Loggable +public class VirtualEncoder implements Encoder { + + @Log(as = "rawValue", atLevel = LogLevel.IMPORTANT) + private double value; + + /** + * Set this encoder's value to an arbitary number. + * + * @param value The raw value to set the encoder to. + */ + public void set(double value) { + this.value = value; + } + + @Override + public double get() { + return value; + } + + @Override + public double countsPerRevolution() { + return 4096; + } +} diff --git a/misc/virt/src/main/java/net/bancino/robotics/swerveio/module/VirtualSwerveModule.java b/misc/virt/src/main/java/net/bancino/robotics/swerveio/module/VirtualSwerveModule.java new file mode 100755 index 0000000..e02742a --- /dev/null +++ b/misc/virt/src/main/java/net/bancino/robotics/swerveio/module/VirtualSwerveModule.java @@ -0,0 +1,70 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.module; + +import net.bancino.robotics.swerveio.geometry.Length; +import net.bancino.robotics.swerveio.geometry.Length.Unit; + +import net.bancino.robotics.swerveio.VirtualMotor; +import net.bancino.robotics.swerveio.encoder.VirtualEncoder; + +/** + * A virtual swerve module that implements the SwerveIO swerve module interface. + * THis does not map to any hardware so it can be used for testing. The methods + * in this interface automatically updates the encoders based on the motor + * speeds. + * + * @author Jordan Bancino + * @version 6.1.0 + * @since 6.1.0 + */ +public class VirtualSwerveModule extends GenericSwerveModule { + + /** + * Create an virtual swerve module with arbitrary parameters using virtual + * motors and encoders. + */ + public VirtualSwerveModule() { + super(new GenericSwerveModule.Builder().setDriveMotor(new VirtualMotor()).setDriveEncoder(new VirtualEncoder()) + .setPivotMotor(new VirtualMotor()).setPivotEncoder(new VirtualEncoder()).setAngleOffset(0) + .setDriveGearRatio(10).setPivotGearRatio(5).setDriveMaxRPM(5000) + .setWheelDiameter(new Length(4, Unit.INCHES))); + } + + @Override + public void setAngle(double angle) { + super.setAngle(angle); + + /* Update the encoder based on the motor speed. */ + VirtualEncoder encoder = (VirtualEncoder) getPivotEncoder(); + encoder.set(encoder.get() + (encoder.countsPerRevolution() * getPivotMotor().get())); + } + + @Override + public void setSpeed(double speed) { + super.setSpeed(speed); + + /* Update the encoder based on the motor speed. */ + VirtualEncoder encoder = (VirtualEncoder) getDriveEncoder(); + encoder.set(encoder.get() + (encoder.countsPerRevolution() * getDriveMotor().get())); + } + + @Override + public double getOutputThreshhold() { + return 0.0; + } +} diff --git a/settings.gradle b/settings.gradle new file mode 100755 index 0000000..9fae3df --- /dev/null +++ b/settings.gradle @@ -0,0 +1,39 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +rootProject.name = 'SwerveIO' + +/* + * The Maven artifact name is the project name. This allows us + * to include subprojects with a given name, to make the Maven artifacts + * more descriptive and user-friendly. + */ +def includeAs = { String proj, String name -> + include proj + project(proj).name = "${rootProject.name}-${name}" +} + +/* Hardware Vendors */ +includeAs(':vendor:ctre', 'CTRE') +includeAs(':vendor:kauai', "KauaiLabs") +includeAs(':vendor:rev', "RevRobotics") + +/* Module Kits */ +includeAs(':kit:sds-mk2', 'SDS-MK2') +includeAs(':kit:sds-mk3', 'SDS-MK3') + +/* Misc. addons/plugins */ +includeAs(':misc:virt', "Virtual") diff --git a/src/javadoc/overview.html b/src/javadoc/overview.html new file mode 100755 index 0000000..e6dee5c --- /dev/null +++ b/src/javadoc/overview.html @@ -0,0 +1,744 @@ + + + + + SwerveIO User Documentation Overview + + + +

User Documentation

+

+ SwerveIO is an open-source swerve drive library written for FRC in Java. It is pronounced "Swerve - ee - oh", + which rhymes with "Oreo". You are currently viewing the official, complete user documentation for this release. + Developer documentation is available on the project source page. +

+

+ SwerveIO is intended to be capable of driving every FRC swerve drive that exists. It features an extremely + standard collection of interfaces that all modules, PID controllers, and encoders must adhere to. This makes + porting your robot code from swerve drive to swerve drive incredibly simple. +

+

+ If you need additional help beyond this documentation, please reach out! I would be more than willing to help + you and your team. You can always + email me at jordan@bancino.net. +

+

Table of Contents

+ +

Introduction

+

+ SwerveIO aims to be as simple and easy to use as possible, yet also powerful, so that rookies and veterans alike + can get the most out of their swerve drive. Everything in this library can be extended and built upon to fit the + widest variety of needs. SwerveIO handles all the complex math in an object-oriented way that Java programmers + should be familiar with. All you need to do is describe your hardware setup, and SwerveIO will do the rest. +

+

+ SwerveIO is extremely modular. As you will notice, there are many packages, classes and interfaces that make + up the entire SwerveIO API. It is designed to be extensible both in it's current form, and as development + progresses. The design goals of SwerveIO are as follows: +

+
    +
  • To provide an easy start for swerve drive beginners
    • +
  • To allow customization at levels not found in other swerve drive libraries
    • +
  • To give complete control over the swerve drive to the programmer, no matter his/her level of experience +
    • +
  • + To provide a feature-complete API that handles everything from the motors and encoders to a functional + autonomous. +
    • +
+

+ Whether you're a first-year student, or 11th-year mentor, SwerveIO is designed to meet your + needs. The internals of the API are masked by default so as not to scare rookies off, but can be easily exposed + and customized by experienced programmers. The target audience of this documentation is anyone interested in + learning how to program an FRC swerve drive with SwerveIO. Ultimately, the math and science behind swerve drive + is beyond the scope of this documentation, though the kinematics package does provide a brief overview of why + things work the way they do, and links to documents containing the math that is implemented in this library. +

+

+ This documentation is standard JavaDoc documentation, so please familiarize yourself with JavaDoc before + continuing. This documentation assumes you are profficient in the Java programming language, the JavaDoc + documentation system, and a build system with which you can comfortably build and deploy Java-based + projects. A basic understanding of the git version control system may also be required for some sections of this + document. Each component of the SwerveIO API will have thorough comments on the relevant pages of + this documentation. +

+

Features

+

+ SwerveIO +

+
    +
  • is expandable: A collection of interfaces allow the use of any motor controllers and + encoders, in any combination. All the classes used in this library are open and can be used independently if + desired, + such as DefaultSwerveKinematics or the PID controller (DefaultPIDController). + Almost every component of SwerveIO can be replaced with custom implementations as well.
    • +
  • has sensible defaults: SwerveIO strives to provide swerve module implementations + for popular hardware configurations, including REVRobotics NEOs and CTRE Falcon 500s. If Team 6090 has + experience with it on swerve, there will be a working implementation here. You're also more than welcome to + add your own module implementations to this library if you wish to officially support them. Please don't add + custom modules, but if it's a kit, by all means, please help support it!
    • +
  • is written in Java: Written in Java by Java developers, SwerveIO takes advantage of the + Java language and follows all the conventions of Java libraries. This makes for seamless integration with + your Java robotics projects.
    • +
  • + is simple: All the hard work is done beneath the abstraction layers of this library, so all + you need to do is describe the hardware using the declarative API. +
    • +
  • + offers automatic control: Just input your swerve drive specs. SwerveIO performs all the + calculations needed for driving, and runs it's own control loops. +
    • +
  • provides an advanced logging API: SwerveIO uses a home-grown logging library to log the + complete state of the swerve drive as it moves. This API allows you to output data to either a file, the + NetworkTables, a database, a server, or more! Default implementations include: +
      +
    • DashboardLog: Log all swerve data to the dashboard so that they can + be viewed live as you're running.
      • +
    • JSON Logger: Log swerve data to a JSON file that can then be parsed and graphed if + desired. +
      • +
    • Build your own: Take a look at the default implementations to give you some + inspiration, then write your own logger, and if you'd like, submit it to the SwerveIO project.
      • +
    +
    • +
+

Dependencies

+

+ The main SwerveIO API, called SwerveIO Core, is very lightweight. There are no dependencies beyond the official + FRC WPILib project. There is no reliance on any other third-party libraries for implementing any part of + SwerveIO; SwerveIO simply supplements the dependencies listed here. As of this release, the complete list of + dependencies that SwerveIO declares is as follows: +

+
    +
  • + WPILib — SwerveIO depends on various parts of the WPILibJ library, including but not + limited to the following components: +
      +
    • + The "new" command framework provided by the edu.wpi.first.wpilibj2 package +
      • +
    • + The driver station and dashboard +
      • +
    • + The kinematics and trajectory packages +
      • +
    • + The filesystem (for PathWeaver trajectories) +
      • +
    • + Various components like speed controllers and encoders +
      • +
    +
    • +
  • + Vendor libraries for driving supported hardware. +
    • +
+

Compatibility

+

This release of SwerveIO is intended to be compatible with the following software:

+
    +
  • WPILib 2022.1.1+
    • +
  • Gradle 7.3.3+
    • +
  • Java 11.0.12+
    • +
+

+ This release of SwerveIO is intended to be compatible with the hardware implementations + provided by the official hardware artifacts. All the subpackages of SwerveIO Core provide generic interfaces + that you can use to support your own hardware. If support for your hardware is not provided by an official + artifact, adding support for it is trivial and can be accomplished by any Java developer using this + documentation. One can easily add custom motor controllers, encoders, PID controllers, loggers, and more using + the SwerveIO interfaces present and documented in their relevant subpackages. +

+

+ For a list of kit assemblies that are officially supported by SwerveIO, see the Supported Hardware section. If you don't see your kit listed, supporting it + should be easy, but feel free to reach out to us so we can help you. +

+

Organization

+ SwerveIO's API is structured in such a way that the SwerveDrive class is the high-level + glue that binds the following lower-level components together: +
    +
  1. + Swerve Module — A collection of two motor controllers and two encoders. Modules are also + responsible for providing position loop controllers to achieve the angles assigned to them. Hardware + implementations are provided by external artifacts not included with SwerveIO Core. +
    2. +
  2. + Kinematics Provider — Vector computation. Given a a 3-dimensional input, + the kinematics provider generates a vector for every swerve module. +
    3. +
  3. + Gyro — The gyro is optional, but it provides field-centric navigation and closed-loop anglular + control if it is present. Gyro support is provided by external artifacts not included with SwerveIO Core. +
    4. +
+

+ SwerveIO also provides some additional higher-level features as well: +

+
    +
  • + Commands — Commands utilize the swerve drive API to provide fully-functional + waypoint autonomous using WPILib Pathweaver, and a joystick command that generates vectors + to provide an intuitive operator interface. +
    • +
  • + Loggers — SwerveIO's LogHandler interface is intuitive and helpful in debugging. + It allows you to monitor the state of the swerve drive over time. You can log to files, the dashboard, and + more. +
    • +
+

+ SwerveIO embraces "functional programming", which allows the programmer to pass a function in + as a parameter to a method or constructor. Functional programming is an efficient way program, + and it may not be possible to avoid it when working with the SwerveIO API. Please make sure + you understand functional programming in Java, and be familiar with lambda expressions, as these + greatly simplify the code that needs to be written. Some of the design choices that were made were done so to + provide a declarative API with which you simply describe your swerve drive configuration. +

+

Getting Started

+

+ This documentation is not intended to be read sequentially, but it would be adventagous to take + a top-down approach to it. Begin with the base package, the commands, and the loggers, all of + which make up the high-level API, then work your way down to the kinematics provider and module + interfaces. It is necessary to have a deep understanding of the way SwerveIO works and how it + is used by the high-level programmer before any attempt to extend hardware support or other + low-level functionality is made. If you are new to SwerveIO, it would be beneficial to start + with the + SwerveDrive.Builder + class documentation, available in the + net.bancino.robotics.swerveio + package. That page will explain what is necessary to create a swerve drive subsystem. Next, + it would be wise to check out the command package, available in + + net.bancino.robotics.swerveio.command, + which contains code for driving the swerve drive with a joystick and autonomously. +

+

+ If you aren't obtaining SwerveIO from a repository using the standard WPILib vendor library procedure, you can + use SwerveIO in a Gradle composite build. Note that there are no official binary distributions of SwerveIO; it + is only available as source code. If you are using a binary distribution, refer to the distribution + documentation to use SwerveIO with your project. To use SwerveIO in a Gradle composite build, download and + extract the code to a subdirectory of your robot code.. Then add the following line to your + settings.grade to include the local copy of SwerveIO: +

+ 
+includBuild "SwerveIO-vX.X.X"
+
+

+ Note that X.X.X is the version of SwerveIO you downloaded. You may want to build your robot code + with the following command to ensure it works properly: +

+ 
+$ gradle build
+
+

+ Note that using a composite build still requires you to have the vendor dependency files installed, or you'll + have to manually add the maven artifacts to your build.gradle. +

+

+ If you're at a competition, or just want faster builds, after running the above command to ensure all + dependencies are resolved, you can add the --offline flag to disable all build features that + require the internet. +

+

+ This documentation assumes that you have successfully configured SwerveIO for use in your robot code + by adding it to your build system of choice. SwerveIO is a Gradle + project, which uses the Maven repository model. FRC switched + from Apache Ant to Gradle a few years ago. Whatever your build + system, it is assumed that you have configured it to allow the use of SwerveIO. This documentation + only details the SwerveIO API. Anything upon which the understanding and usage of this API depends + is considered a prerequisite that is to be fulfilled by the reader. +

+

Supported Hardware

+

+ Starting with SwerveIO 6.0.0, the entire SwerveIO API is broken up into three types of components: +

+
    +
  • + SwerveIO Core — The main API. This contains all the hardware interfaces, the main + SwerveDrive subsystem, and commands that drive the SwerveIO drive system. +
    • +
  • + Hardware Vendors — Support for encoders and motor controllers. These components bind the + hardware vendor APIs to the SwerveIO API and are the mediators between the SwerveIO core API and the kit + implementations. +
    • +
  • + Kits — Kits use the hardware vendor APIs and provide constants such as gear ratios and motor + rotations These components also provide sensible tuning parameters as defaults. +
    • +
+

+ Each component is maintained as a Gradle subproject in the SwerveIO repository and may be published as a + separate Maven artifact. Below is an exhaustive list of all the officially supported hardware. You can add this + hardware support to your code by dropping the vendor JSON files generated by the publish Gradle + task into your vendordeps folder. +

+

Hardware Vendors

+ + + + + + + + + + + + + + + + + + +
Supported Hardware Vendors
VendorHardware
Cross The Road ElectronicsAll motor-attached encoders, and the CANCoder
RevRoboticsSpark Max-attached encoders
Kauai LabsNavX Gyro
+

Kits

+ + + + + + + + + + +
Supported Kits table
VendorKits
Swerve Drive SpecialtiesMK2, MK3
+

+ These kits will automatically pull in the required vendor dependencies, so you don't have to include hardware + libraries explicitly unless you're using them outside of the kit implementation. +

+

+ If you want to mock up a swerve drive configuration for unit testing or other experimental purposes, you can + pull in the SwerveIO-Virtual project, which provides a swerve module implementation that is not + tied to any hardware at all. +

+

Terminology

+

+ There are a number of different terms that this library uses. It is important to understand them + to understand how this library works. The terms used throughout this documentation and the SwerveIO + code are listed below, and defined how they are used in this library. +

+
    +
  • + A swerve module is a single unit of the swerve drive, comprised of a pivot motor and a drive motor + attached to a gearbox and a wheel. A SwerveIO swerve drive is made up of exactly four swerve modules. +
    • +
  • + Field-Centric Navigation refers to using a gyroscope to determine the orientation of the + robot and driving in a consistent way. This is commonly used when the driver is not inside + the swerve vessel because it allows the driver to drive the robot without any regard to the + orientation of it. Due to the nature of swerve drive, the robot rotates a lot, so it can be + a burden on the driver to conciously recall the "front" orientation in order to properly drive + it. By using field-centric navigation, the robot is driven the same way no matter it's orientation. + When configured properly, whether the robot is forward, backward, or at any angle in between, + pressing the joystick away from the driver will result in the robot moving away from the + driver. +
    • +
  • + Pivot is the term used to describe the motor and motion of the individual swerve modules + when they rotate in place. Some refer to this as "azimuth", but "pivot" is easier to say and + is thus more likely to be discussed. The word "steering" may also be used in place of "pivot", though + "pivot" is more appropriate because the pivot motors do not "steer" the swerve drive in any sense, they + merely "pivot" the drive wheels. +
    • +
  • + A vector in SwerveIO is an multi-dimensional data object. Vectors contain a magnitude and + a direction, but magnitudes and directions may be represented differently. In the case of + SwerveVector, direction is indicated with the sign of the values, but ModuleVector + has an angle field that represents the direction. It all depends on the use of the vector. All + vectors also have very specific bounds, so please read the documentation on them before using them. +
    • +
+ +

Appendix

+

+ This appendix provides small help documents to get you started working with SwerveIO. Everything covered in this + appendix is available in the standard Java documentation, but these entries are designed to provide "tips and + tricks" for using SwerveIO. Things that may not be immediately obvious from reading the documentation but are + common enough tasks are documented here. +

+

Adding New Hardware

+

+ Adding new hardware support for SwerveIO is quite simple using GenericSwerveModule. + GenericSwerveModule allows you to declare the hardware properties of your swerve module. + The point of adding hardware-specific classes to SwerveIO is to provide a convenient place for tuning parameters + and direct hardware configuration. End users should be able to simply instantiate a new module object, say + MK3SwerveModule without caring what the gear ratios or PID parameters are. Hardware-specific swerve + module classes make this possible by hard-coding in all the required properties such as gear ratios, motor RPMS, + and wheel diameters. It is also highly recommended to provide sensible default PID tuning parameters. Any + module-specific initialization that needs to occur (such as reversing the direction of a motor) should also + happen here so that users can simply instantiate the module and start running. +

+

+ SwerveIO provides "official" support for multiple swerve modules, which means that all the numbers have been + crunched and hard-coded for you if you choose to use those modules. If you choose not to use "official" modules, + you'll have to write your own class for them. This appendix entry aims to assist you in this task, which is easy + from a programming perspective, but can be difficult because there are lots of numbers that need to be + calculated or discovered by experimentation. +

+

+ At the very least, you'll need the following measurements: +

+
    +
  • + The drive gear ratio (input / output) +
    • +
  • + The drive motor's maximum theoretical (or actual) rotations per minute. +
    • +
  • + The module wheel's diameter. +
    • +
+

+ You also need to consider your encoder source. If you are not using an external 1:1 encoder for the pivot + motor, you must also provide a pivot gear ratio. This most likely isn't documented well if you're using a kit, + because kit builders often push you to use the 1:1 encoder, so you may need to verify this ratio experimentally. +

+

+ If you're using an integrated encoder, you may need to reverse the direction of the pivot motor by calling + setInverted() on it. If you get your swerve drive running but the wheels don't go to the correct + angles, try this before changing anything else. +

+

+ Regardless of what encoder you're using, be it external or integrated, it has to implement the + Encoder interface. The Encoder interface is a simple wrapper for hardware + vendor-specific encoder reading classes that enables any encoder to be used with SwerveIO. See the Encoder documentation for the + specifications. SwerveIO already has support for multiple encoders, so check the encoder package before + implementing the interface for yourself. +

+

+ Once you have all the required information, as well as an Encoder class, create a new class that + extends GenericSwerveModule. Consider accepting the following in your constructor: +

+
    +
  • + The drive motor CAN ID. +
    • +
  • + The pivot motor CAN ID. +
    • +
+

+ These things will differ from robot to robot and even module to module, so you don't want to make any + assumptions on them. You may also consider accepting a pivot encoder in your constructor as well if you aren't + using the encoder integrated into the pivot motor (if equipped). If you want to support both CAN and PWM, you + may also provide constructors for these, or add an enumeration to allow users to specify the mode of operation. +

+

+ The next step is to call the super constructor with an annonymous builder object. This is precisely what the + "offical" modules do. Consider the following example: +

+ 
+import net.bancino.robotics.swerveio.module.GenericSwerveModule;
+import net.bancino.robotics.swerveio.geometry.Length;
+import net.bancino.robotics.swerveio.geometry.Length.Unit;
+
+public class MySwerveModule extends GenericSwerveModule {
+    public MySwerveModule(int driveCanId, int pivotCanId) {
+        super(new GenericSwerveModule.Builder()
+            .setDriveMotor(/* Create your drive motor here */, (builder, motor) -> {
+                /* Configure/initialize the drive motor in this block */
+                builder.setDriveEncoder(/* Create your drive encoder here */);
+            })
+            .setPivotMotor(/* Create your pivot motor here */, (builder, motor) -> {
+                /* Configure/initialize the pivot motor in this block */
+                builder.setPivotEncoder(/* Create your pivot encoder here */);
+            })
+
+            /* Declare the module specifications */
+            .setDriveGearRatio(/* Set the drive gear ratio here */)
+            .setPivotGearRatio(/* Set the pivot gear ratio here */) /* Only required if encoder isn't 1:1 */
+            .setWheelDiameter(/* Set the wheel diameter here */)
+            .setMaxDriveRPM(/* Set the drive motor's max drive RPMs here */)
+        );
+    }
+}
+
+

+ It is highly recommended to follow the constructor conventions of the modules implemented in this class, so you + don't confuse other potential users. If you did everything properly, that is a complete swerve module + implementation. As you can see, it declaratively configures and initializes the hardware. SwerveIO will take + care of the rest. You should now be able to add instances of your swerve module implementation to the SwerveIO + module map. See Show Me Code and SwerveDrive.Builder for more details. +

+

Finding Angle Offsets

+

+ Generating the swerve module angle offsets necessary for the pivot functionality can be the most difficult + part of setting up your swerve drive. This appendix entry provides detailed instructions for successfully + completing this task with SwerveIO. These instructions only apply for absolute encoders. If you use + relative encoders, you will have to align your swerve drive each time you power on your robot. +

+

+ The first step is to ensure that your swerve modules are placed in their forward position. This means making + sure all the wheels are aligned and pointed to the front of your robot. You can use a custom alignment frame, or + a couple pieces of long wood, metal, or pipe. Team 6090 uses thin blocks of metal that are long enough to reach + across both modules on one side. We first rotate the modules by hand until they're generally in the forward + position, then we press the long metal into both wheels, ensuring that they are even with each other. We then + repeat on the other side of the swerve chassis. +

+

+ As of SwerveIO 6.1.0, you no longer need to manually calculate the angle offsets. At this point, once you're + aligned, you can call saveAngleOffsets(), either on + code initialization or via a command—see the SaveSwerveAngleOffsets command. It's up to you + how you want to do this. Team 6090 recommends mapping a + dashboard button for this purpose, and this functionality is provided in the SaveSwerveAngleOffsets + command. After the initial save, you can instruct SwerveIO to initialize with these + offsets using loadAngleOffsets(). +

+

+ If you wish to manually set the offsets, you need some kind of feedback to the driver station. See Shuffleboard for instructions on using Shuffleboard to get information on your + swerve drive. +

+

+ In your robot code, ensure that no existing offsets are applied. This means setting all your module offsets to + zero. This ensures that you are getting an accurate angle reading. If all your modules are aligned, write down + the current angle readings for each module. Be as precise as possible. These angles are arbitrary because they + are calculated from the encoder, which is most likely not "zero" at the forward position, which is why this + angle offset is necessary The current reported angle of the swerve module when it is aligned is the offset + for that module. You can provide these numbers wherever an "angle offset" value is required. +

+

+ You don't have to perform this process every time you use your robot; absolute encoders store their position + across power losses, so your swerve drive will remember these angles. The offset basically just tells SwerveIO + the encoder reading at which each swerve module is in it's forward position. You will have to repeat this + process every time you disassemble the swerve module, because the motor, motor shaft, gears, or wheel alignment + may be modified during assembly or repairs. Team 6090 also "re-calibrates" the swerve drive before every + competition to ensure accuracy. +

+

Vendor-Specific Tuning

+

+ Wherever possible, SwerveIO provides a unified interface for all vendor-specific classes and methods. This + method is not flawless, because often features don't line up exactly, which can result in some PID controller + methods throwing an exception if a particular feature isn't supported by the underlying vendor library. On the + flip side, sometimes vendors offer functionality not explicitly required for SwerveIO's operation, which means + these features are inaccessible through the SwerveIO interfaces. Luckily, it is not that difficult to access + these vendor-specific features through casting. This requires knowledge of the underlying vendor libraries and a + particular swerve module implementation. If needed, please take a look at the source code so you can understand + how the following example works in order to apply it to your own situation. +

+

+ Consider that you are using MK3SwerveModules and you are running a velocity loop on the drive + motors. You can set PIDF values using SwerveIO's PhoenixPIDController, but that only provides basic + configuration. The CTRE Phoenix library provides a large amount of configuration options that SwerveIO can't + possibly hope to provide support for. To access these options, familarize yourself with + SwerveDrive.Builder's setModuleMap() + methods. One implementation of setModuleMap() takes a module initializer, which can be used to + perform additional initialization on a swerve module before passing it to the swerve drive controller. It is + through this method that you can access all the methods of the SwerveModule interface, and thus + access vendor-specific methods as well. +

+

+ Here's an example snippet. Refer to Show Me Code to see where this fits into your + swerve drive setup code. +

+ 
+/* swerve is a reference to a SwerveDrive */
+swerve.forEachModule((location, module) -> {
+    /* The code in this block is run on all the swerve modules in the module map. */
+
+    /* Use SwerveIO interface methods to set tuning parameters. This will run on any module. */
+    PIDController pid = module.getPivotPIDController();
+    pid.setP(0.1);
+    pid.setI(0.001);
+    pid.setD(0);
+
+    /* Use vendor-specific method via casting. This will only run on modules that use TalonFX controllers. */
+    WPI_TalonFX driveMotor = (WPI_TalonFX) module.getDriveMotor();
+    driveMotor.clearStickyFaults();
+    driveMotor.setIntegralAccumulator(0.001);
+});
+
+

+ We are most interested in the last few lines. Here, you can see that we cast the swerve module's drive motor to + a WPI_TalonFX (remember that we're using an MK3SwerveModule, which uses those motor + controllers) to access additional configuration parameters. This is the easiest way to set custom configuration + parameters on vendor-specific hardware for every swerve module. This method depends on you knowing the + underlying motor controller implementation being used by a swerve module. This information is all documented in + the respective documentation. +

+

Show Me Code!

+

+ It's up to you to read the relevant documentation for getting SwerveIO up and running, but if you're really + struggling putting the pieces together, here's an example that uses SwerveIO with MK3SwerveModule. + You should get the general idea, and indeed you should be able to copy-paste this example, making only minor + adjustments, but it is not recommended you do so because you won't learn how SwerveIO works. This listing is for + reference only, and even though you may use a different setup, you'll find this to be a good template. Also + remember that this is not a complete reference; it does not showcase all SwerveIO features in the slightest. For + that you'll have to read the documentation. +

+ 
+package frc.robot.subsystems;
+
+import net.bancino.robotics.swerveio.SwerveDrive;
+import net.bancino.robotics.swerveio.module.SwerveModule;
+import net.bancino.robotics.swerveio.module.MK3SwerveModule;
+import net.bancino.robotics.swerveio.gyro.Gyro;
+import net.bancino.robotics.swerveio.geometry.Length;
+import net.bancino.robotics.swerveio.geometry.Length.Unit;
+import net.bancino.robotics.swerveio.geometry.ChassisDimension;
+import net.bancino.robotics.swerveio.pid.PIDController;
+import net.bancino.robotics.swerveio.log.DashboardLog;
+
+/**
+ * The drivetrain subsystem drives the robot. Call create() to
+ * get a new instance of a SwerveIO drivetrain subsystem.
+ *
+ * This subsystem consists of the following components:
+ * - Swerve module (4x drive + pivot motor)
+ *
+ * This subsystem should provide an interface for the
+ * following functions:
+ * 
+ * - Running the drivetrain with joystick
+ * - Running the drivetrain autonomously
+ *
+ * @author Jordan Bancino
+ */
+public class DriveTrain {
+
+  /* Your robot chassis dimensions - measured from wheel to wheel. */
+  private static final ChassisDimension CHASSIS_DIMENSIONS = new ChassisDimension(new Length(22.5, Unit.INCHES));
+
+  /* CAN IDs - Set these according to your configuration in Phoenix Tuner */
+
+  private static final int FRONT_RIGHT_DRIVE_MOTOR =  5; /* Module 1 */
+  private static final int FRONT_LEFT_DRIVE_MOTOR  =  6; /* Module 2 */
+  private static final int REAR_LEFT_DRIVE_MOTOR   =  7; /* Module 3 */
+  private static final int REAR_RIGHT_DRIVE_MOTOR  =  8; /* Module 4 */
+
+  private static final int FRONT_RIGHT_PIVOT_MOTOR =  1; /* Module 1 */
+  private static final int FRONT_LEFT_PIVOT_MOTOR  =  2; /* Module 2 */
+  private static final int REAR_LEFT_PIVOT_MOTOR   =  3; /* Module 3 */
+  private static final int REAR_RIGHT_PIVOT_MOTOR  =  4; /* Module 4 */
+
+  private static final int FRONT_RIGHT_ENCODER     =  9; /* Module 1 */
+  private static final int FRONT_LEFT_ENCODER      = 10; /* Module 2 */
+  private static final int REAR_LEFT_ENCODER       = 11; /* Module 3 */
+  private static final int REAR_RIGHT_ENCODER      = 12; /* Module 4 */
+
+  /* Angle offsets in degrees - See SwerveIO appendix for setting these. */
+
+  public static final double FRONT_RIGHT_ANGLE_OFFSET = 146.27;
+  public static final double FRONT_LEFT_ANGLE_OFFSET  = 134.60;
+  public static final double REAR_LEFT_ANGLE_OFFSET   = 59.34;
+  public static final double REAR_RIGHT_ANGLE_OFFSET  = 267.2;
+
+  public static SwerveDrive create(Gyro gyro) {
+    return new SwerveDrive.Builder()
+      .useDefaultKinematics(CHASSIS_DIMENSIONS)
+      .setGyro(gyro)
+      .setModuleMap((map) -> {
+        map.put(SwerveModule.Location.FRONT_RIGHT, new MK3SwerveModule(
+            FRONT_RIGHT_DRIVE_MOTOR, FRONT_RIGHT_PIVOT_MOTOR,
+            FRONT_RIGHT_ENCODER, FRONT_RIGHT_ANGLE_OFFSET
+        ));
+        map.put(SwerveModule.Location.FRONT_LEFT, new MK3SwerveModule(
+            FRONT_LEFT_DRIVE_MOTOR, FRONT_LEFT_PIVOT_MOTOR,
+            FRONT_LEFT_ENCODER, FRONT_LEFT_ANGLE_OFFSET
+        ));
+        map.put(SwerveModule.Location.REAR_LEFT, new MK3SwerveModule(
+            REAR_LEFT_DRIVE_MOTOR, REAR_LEFT_PIVOT_MOTOR,
+            REAR_LEFT_ENCODER, REAR_LEFT_ANGLE_OFFSET
+        ));
+        map.put(SwerveModule.Location.REAR_RIGHT, new MK3SwerveModule(
+            REAR_RIGHT_DRIVE_MOTOR, REAR_RIGHT_PIVOT_MOTOR,
+            REAR_RIGHT_ENCODER, REAR_RIGHT_ANGLE_OFFSET
+        ));
+      })
+      .build((swerve) -> {
+          swerve.getLogger().outputTo(new DashboardLog());
+      });
+  }
+}
+
+

+ If you're looking for more examples, check out Team 6090's Github + Organization. There we have a SwerveIOTestBase and our InfiniteRecharge code, + both of which utilize SwerveIO. It is up to you to integrate SwerveIO into your robot code; that is beyond the + scope of this documentation. We find that this static create() method works the best for us because + it isolates our swerve drive configuration in its own file or method, but you may use SwerveIO however works + best for you. SwerveIO is designed to be usable in a wide variety of configurations. While it was designed to be + command-based, there is certainty nothing stopping anyone from using it in other ways. +

+

Dashboard

+

+ You can have SwerveIO automatically output a plethora of information on your swerve drive using the + DashboardLog. This class writes useful information—including encoder positions and module + angles—to the Smart Dashboard. To enable this feature on your swerve drive, use the following code: +

+ 
+import net.bancino.robotics.swerveio.SwerveDrive;
+import net.bancino.robotics.swerveio.log.DashboardLog;
+/* ... */
+SwerveDrive swerve = /* Fetch or create your swerve drive here. */;
+swerve.getLogger().outputTo(new DashboardLog());
+
+

+ You simply call outputTo() on your swerve drive's logger with a new DashboardLog + object. When your robot code runs, you should see live feedback from your swerve drive under the "SwerveIO" + section of your dashboard. +

+

License

+

+ Copyright (C) 2019-2021 Jordan Bancino <jordan@bancino.net> +

+

+ All parts of SwerveIO, including the source code, build scripts, and documentation, are licensed under the GNU + GPL V3. Please read it carefully before using SwerveIO for any purpose. While you do have the right to use and + modify SwerveIO for most purposes, you absolutely cannot use SwerveIO in proprietary software. Any software that + uses SwerveIO or a derivative of SwerveIO MUST be open source in the sense that anyone can easily obtain + the source code that uses SwerveIO. Furthermore, any derivative of SwerveIO must also be open source and bound + by the GNU GPL V3. This is to foster an open development environment. SwerveIO is graciously released free of + charge to the community, so please adhere to the terms of the GNU GPL V3, and before forking SwerveIO, consider + contributing to the upstream project. I want SwerveIO to be the best that it can be for everyone, so if you have + an improvement to share, it would be greatly appreciated if you offered it to the main project instead of making + the changes privately in your own fork. +

+

+ You must not remove any copyright notices that appear in this code, either in comments or in console output. +

+ +
+ + + diff --git a/src/main/java/net/bancino/log/JsonLog.java b/src/main/java/net/bancino/log/JsonLog.java new file mode 100755 index 0000000..f12c716 --- /dev/null +++ b/src/main/java/net/bancino/log/JsonLog.java @@ -0,0 +1,118 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.log; + +import java.io.File; +import java.io.FileNotFoundException; +import java.io.PrintStream; +import java.util.List; + +/** + * A log hander that generates JSON output directly mirroring + * the code hierarchy. + * + * @author Jordan Bancino + * @version 7.0.0 + * @since 7.0.0 + */ +public class JsonLog implements LogHandler { + + public static String toJson(LogEntry entry) { + StringBuilder json = new StringBuilder(); + if (entry != null) { + switch (entry.getType()) { + case LIST: + List values = entry.getAsList(); + json.append('['); + for (int i = 0; i < values.size(); i++) { + json.append(toJson(values.get(i))); + if (i < values.size() - 1) { + json.append(','); + } + } + json.append(']'); + break; + case NODE: + json.append('{'); + for (int i = 0; i < entry.getChildren().size(); i++) { + LogEntry child = entry.getChildren().get(i); + json.append('"').append(child.getName()).append("\":"); + json.append(toJson(child)); + if (i < entry.getChildren().size() - 1) { + json.append(','); + } + } + json.append('}'); + break; + case VALUE: + Object raw = entry.getValue(); + String str; + + if (raw == null) { + str = "null"; + } else if (raw instanceof Number || raw instanceof Boolean) { + str = raw.toString(); + } else { + /* TODO: do proper JSON escaping. */ + str = String.format("\"%s\"", raw.toString()); + } + json.append(str); + break; + default: + break; + } + } + return json.toString(); + } + + private final PrintStream out; + private boolean doComma = false; + + public JsonLog(String out) throws FileNotFoundException { + this(new File(out)); + } + + public JsonLog(File out) throws FileNotFoundException { + this(new PrintStream(out)); + } + + public JsonLog(PrintStream out) { + this.out = out; + } + + @Override + public void open() { + out.print('['); + } + + @Override + public void accept(LogEntry entry) { + String json = toJson(entry); + if (doComma && !json.isBlank()) { + out.print(','); + } else { + doComma = true; + } + out.print(json); + } + + @Override + public void close() { + out.println(']'); + out.close(); + } +} diff --git a/src/main/java/net/bancino/log/Log.java b/src/main/java/net/bancino/log/Log.java new file mode 100755 index 0000000..3bc1fd8 --- /dev/null +++ b/src/main/java/net/bancino/log/Log.java @@ -0,0 +1,53 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.log; + +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; + +/** + * The Log annotation. This is used to annotate fields that should have their values + * logged. LogIO will automatically collect primitives and basic object information. + * If a field is of a custom type, make sure that custom type is annotated with + * {@link Loggable} to ensure that it actually produces useful log information. + * + * @author Jordan bancino + * @version 7.0.0 + * @since 7.0.0 + */ +@Retention(RetentionPolicy.RUNTIME) +@Target(ElementType.FIELD) +public @interface Log { + + /** + * Set the key name for this field. By default, the key name is the name of + * the field variable, but this can be used to change that if the field variable + * name is not appropriate or descriptive enough. + * @return The key name for this field, or a blank string if the name of the field + * variable should be used. + */ + public String as() default ""; + + /** + * Set the level at which this field should be logged. + * @return The log level at which this field should be logged. + * The default is {@code TRACE}. + */ + public LogLevel atLevel() default LogLevel.TRACE; +} diff --git a/src/main/java/net/bancino/log/LogEntry.java b/src/main/java/net/bancino/log/LogEntry.java new file mode 100755 index 0000000..5e9c3df --- /dev/null +++ b/src/main/java/net/bancino/log/LogEntry.java @@ -0,0 +1,180 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.log; + +import java.util.ArrayList; +import java.util.List; + +/** + * A representation of a log enry. LogIO log entries are of a certain type, and + * may optionally have a parent and any number of children. This class creates + * the log tree hierarchy. + * + * @author Jordan Bancino + * @version 7.0.0 + * @since 7.0.0 + */ +public class LogEntry { + + /** + * The possible log entry types. + */ + public static enum Type { + /** + * Indicates a log entry is a list of loggable objects. + */ + LIST, + + /** + * Indicates a log entry is the parent to a number of child + * log entries. + */ + NODE, + + /** + * Indicates a log entry is a value that should be logged. + */ + VALUE + } + + private final LogEntry parent; + private final List children; + private String name; + private Object value; + + /** + * Create a new log entry. This constructor should only be used by + * {@link Logger}. Any other uses may result in undefined behavior. + * + * @param parent The parent log entry, or {@code null} if this is the + * root entry. + * @param name The name or "key" of this entry. + * @param value The value of this entry. + */ + public LogEntry(LogEntry parent, String name, Object value) { + this.parent = parent; + this.children = new ArrayList<>(); + this.name = name; + this.value = value; + + if (parent != null) { + parent.getChildren().add(this); + } + } + + /** + * Get the parent log entry of this log entry. + * + * @return A parent log entry, or {@code null} if this log entry is a root entry. + */ + public LogEntry getParent() { + return parent; + } + + /** + * Get the child log entries of this log entry. + * + * @return A list of child log entries. This may be an empty list if there + * are no children, but it will never be null. + */ + public List getChildren() { + return children; + } + + /** + * Get the name of this log entry. + * + * @return The name of this log entry. + */ + public String getName() { + return name; + } + + /** + * Set the name of this log entry. + * + * @param name The new name of this log entry. + */ + public void setName(String name) { + this.name = name; + } + + /** + * Get the value of this log entry. + * + * @return The raw value of this log entry. + */ + public Object getValue() { + return value; + } + + /** + * Set the value of this log entry. + * + * @param value The new value of this log entry. + */ + public void setValue(Object value) { + this.value = value; + } + + /** + * Get entry type of this log entry. This is determined by evaluating the type of + * the current value at the time of calling, and should be used to guide decisions + * of what to do with the value. + * + *
    + *
  • + * If this returns {@code Type.LIST}, then this log entry's value should be + * processed by calling {@link #getAsList()}. + *
    • + *
  • + * If this returns {@code Type.NODE}, then this log entry's value should be + * processed by calling {@link #getChildren()}. + *
    • + *
  • + * If this returns {@code Type.VALUE}, then this log entry's value should be + * processed by calling {@link #getValue()}. + *
    • + *
+ * + * Any other invocation usage of this log entry object is undefined. + * + * @return The type that this log entry's value represents. + */ + public Type getType() { + if (getValue() != null) { + if (getValue() instanceof List) { + return Type.LIST; + } else if (getValue().getClass().getAnnotation(Loggable.class) != null || getChildren().size() > 0) { + return Type.NODE; + } + } + return Type.VALUE; + } + + /** + * Get the value of this log entry as a list. This will only with if the type + * of this log entry is set to {@code Type.LIST}. + * + * @return The value of this log entry as a list of log entries, or null if this + * log entry does not represent a list. + */ + @SuppressWarnings("unchecked") + public List getAsList() { + return getType() == Type.LIST ? (List) getValue() : null; + } +} diff --git a/src/main/java/net/bancino/log/LogHandler.java b/src/main/java/net/bancino/log/LogHandler.java new file mode 100755 index 0000000..b0b9aae --- /dev/null +++ b/src/main/java/net/bancino/log/LogHandler.java @@ -0,0 +1,52 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.log; + +import java.io.Closeable; +import java.util.function.Consumer; + +/** + * An interface for generating human-readable logs from {@link LogEntry} trees. + * This interface provides a way for LogIO users to generate their own custom + * logs. It is extremely basic in definition, but implementing it can be quite + * tricky, due to the recursive nature of LogIO. To understand a bit more how + * this interface works, you may wish to look at the code for {@link JsonLog}. + * + * @author Jordan Bancino + * @version 7.0.0 + * @since 7.0.0 + */ +/* Consumer provides accept(LogEntry), Closeable provides close() */ +/* This is still technically a functional interface because it only has one method that is mandatory. */ +@FunctionalInterface +public interface LogHandler extends Consumer, Closeable { + /** + * This method is called upon the first invocation of {@link Logger#log(Object)}. + * It can be used for setting up log files, by, for instance, writing log headers + * or other information. + */ + public default void open() { + } + + /** + * This method is called upon the invocation of {@link Logger#close()}. + * It can be used for writing log footers and closing resource streams. + */ + @Override + public default void close() { + } +} diff --git a/src/main/java/net/bancino/log/LogLevel.java b/src/main/java/net/bancino/log/LogLevel.java new file mode 100755 index 0000000..9b056e4 --- /dev/null +++ b/src/main/java/net/bancino/log/LogLevel.java @@ -0,0 +1,65 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.log; + +/** + * An enumeration that defines the support log levels. LogIO allows every field + * to declare the verbosity level at which it is logged. The user can then set the + * log level to include or omit log information depending on his or her needs. + * + * @author Jordan Bancino + * @version 7.0.0 + * @since 7.0.0 + */ +public enum LogLevel { + /** + * Log nothing. This effectively disables LogIO altogether by bypassing all + * the log routines. This can be useful for enhancing the performance of + * production code when no log data is needed. + */ + NOTHING, + + /** + * Log only the most important data. Fields that are set to log at this + * level will be logged every time, no matter the log level, unless it + * is set to {@code NOTHING}. + */ + IMPORTANT, + + /** + * This value indicates a field is informational in nature, and can be useful + * in many scenarios. This value just below {@code IMPORTANT}, which means + * that fields that are set to log at this level will be logged in every scenario + * that the log level is not set to {@code IMPORTANT} or {@code NOTHING}. + */ + INFO, + + /** + * Log most data. This value indicates a verbose log, which can be useful for + * debugging. Fields that are set to log at this level are generally unimportant + * to the normal user and only useful for developers. They are only logged if the + * log level is set to {@code DEBUG} or {@code TRACE}. + */ + DEBUG, + + /** + * Log everything. By default, {@link Log} sets the field log level to {@code TRACE}. + * Setting the log level to {@code TRACE} guarantees that all fields annotated with + * {@link Log} and are reachable from an object are indeed logged. + */ + TRACE +} diff --git a/src/main/java/net/bancino/log/Loggable.java b/src/main/java/net/bancino/log/Loggable.java new file mode 100755 index 0000000..e09871d --- /dev/null +++ b/src/main/java/net/bancino/log/Loggable.java @@ -0,0 +1,32 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.log; + +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; + +/** + * The Loggable annotation indicates that a type is loggable. If this annotation is + * present on a type, LogIO will decend into instances of that type and search for + * fields annotated with {@link Log}. This allows recursive log trees to be constructed. + */ +@Retention(RetentionPolicy.RUNTIME) +@Target(ElementType.TYPE) +public @interface Loggable { +} diff --git a/src/main/java/net/bancino/log/Logger.java b/src/main/java/net/bancino/log/Logger.java new file mode 100755 index 0000000..e671968 --- /dev/null +++ b/src/main/java/net/bancino/log/Logger.java @@ -0,0 +1,227 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.log; + +import java.io.Closeable; +import java.lang.reflect.Array; +import java.lang.reflect.Field; +import java.util.AbstractMap; +import java.util.ArrayList; +import java.util.List; +import java.util.Map; + +/** + * The main LogIO class that is responsible for using reflection to recursively + * process all {@link Log} and {@link Loggable} annotations to generate a tree + * of {@link LogEntry}s. + * + * @author Jordan Bancino + * @version 7.0.0 + * @since 7.0.0 + */ +public class Logger implements Closeable { + + private LogLevel level = LogLevel.INFO; + private List to = new ArrayList<>(); + private boolean calledOpen = false; + + /** + * Get whether or not the field annotated with the provided annotation + * instance should be logged or not. This takes in to account the currently + * set log level, and the log level of the annotation. + * + * @param annot The log annotation to check. + * @return Whether or not the field annotated with this annotation should be + * added to the {@link LogEntry} tree. + */ + private boolean shouldLog(Log annot) { + return level != LogLevel.NOTHING && annot.atLevel().ordinal() <= level.ordinal(); + } + + /** + * Set the log level on this logger. This determines what fields get logged + * and what fields are skipped, based on each field's log level. + * + * @param level The level, above which, fields should be logged at. + * @return This instance, for method chaining. + */ + public Logger setLevel(LogLevel level) { + this.level = level; + return this; + } + + /** + * Add log handers to process the log entries created by this logger. + * + * @param outputs Custom log output handlers. + * @return This instance, for method chaining. + */ + public Logger outputTo(LogHandler... outputs) { + for (LogHandler output : outputs) { + if (output != null && !to.contains(output)) { + to.add(output); + } + } + return this; + } + + /** + * Get the log level that is currently set. + * + * @return The current log level. + */ + public LogLevel getLevel() { + return level; + } + + /** + * Recursively log an object. This method processes all annotated fields + * and generates a log entry tree. It then automatically calls all the + * log handlers that are associated with this class. + * + * @param o The object to log all values for. + * @throws UnsupportedOperationException If a reflection operation fails. + */ + public void log(Object o) throws UnsupportedOperationException { + if (level != LogLevel.NOTHING) { + try { + LogEntry rootEntry = recursiveLog(null, null, o); + for (LogHandler output : to) { + if (output != null) { + if (!calledOpen) { + output.open(); + calledOpen = true; + } + output.accept(rootEntry); + } + } + } catch (IllegalAccessException e) { + throw new UnsupportedOperationException(e); + } + } + } + + private void extractFields(LogEntry parent, Class clazz, Object obj) throws IllegalAccessException { + /* + * If the annotation is not null, the user wants us to log values in this + * object's class, not just the object itself. + */ + if (clazz.getAnnotation(Loggable.class) != null) { + /* Iterate over all the fields of the class */ + for (Field field : clazz.getDeclaredFields()) { + /* Get the field annotation. */ + Log fieldAnnot = field.getAnnotation(Log.class); + + /* + * If the field annotation is not null, the user wants us to log the value of + * this field. + */ + if (fieldAnnot != null && shouldLog(fieldAnnot)) { + /* If the field is private, this will allow us to retrieve it. */ + field.setAccessible(true); + + /* Get the name of the field as it is declared in code. */ + String name = field.getName(); + + /* If the user provided a custom name for this field, use that instead. */ + if (!fieldAnnot.as().trim().isBlank()) { + name = fieldAnnot.as().trim(); + } + + /* Get the field's value on the current object. */ + Object fieldVal = field.get(obj); + LogEntry fieldEntry = recursiveLog(parent, name, fieldVal); + + if (fieldVal == null) { + /* Do nothing, the assignment above appends a null object. */ + } else if (fieldVal instanceof List) { + /* If the field is a list, log each item in the list. */ + List itemEntries = new ArrayList<>(); + for (Object item : (List) fieldVal) { + itemEntries.add(recursiveLog(null, null, item)); + } + fieldEntry.setValue(itemEntries); + } else if (fieldVal.getClass().isArray()) { + /* If the field is an array, log each item in the list. */ + List itemEntries = new ArrayList<>(); + for (int i = 0; i < Array.getLength(fieldVal); i++) { + Object item = Array.get(fieldVal, i); + itemEntries.add(recursiveLog(null, null, item)); + } + fieldEntry.setValue(itemEntries); + } else if (fieldVal instanceof Map) { + /* If the field is a map, log each item in the map. */ + Map fieldMap = (Map) fieldVal; + for (Object k : fieldMap.keySet()) { + Object v = fieldMap.get(k); + recursiveLog(fieldEntry, k.toString(), v); + } + } else if (fieldVal instanceof Enum) { + fieldEntry.setValue(fieldVal.toString()); + fieldEntry.getChildren().clear(); + } + } + } + } + } + + private LogEntry recursiveLog(LogEntry parent, String thisName, Object obj) throws IllegalAccessException { + + /* If the object is null, return a null entry. */ + if (obj == null) { + return new LogEntry(parent, thisName, null); + } + + /* Get this object's class. */ + Class clazz = obj.getClass(); + String className = clazz.getSimpleName(); + + LogEntry entry = new LogEntry(parent, thisName != null ? thisName : className, obj); + + /* Iterate over superclasses */ + Class superClass = clazz.getSuperclass(); + while (superClass != null) { + /* The conditions for including this superclass's fields. */ + boolean include = true; + include &= !superClass.isInterface(); + include &= !superClass.equals(AbstractMap.class); + include &= !superClass.equals(Number.class); + include &= !superClass.equals(Object.class); + if (include) { + LogEntry superclassEntry = new LogEntry(entry, superClass.getSimpleName(), obj); + extractFields(superclassEntry, superClass, obj); + } + superClass = superClass.getSuperclass(); + } + + extractFields(entry, clazz, obj); + return entry; + } + + /** + * Calls {@link LogHandler#close()} on all handlers associated with this + * logger. + */ + @Override + public void close() { + if (level != LogLevel.NOTHING) { + for (LogHandler output : to) { + output.close(); + } + } + } +} diff --git a/src/main/java/net/bancino/log/package-info.java b/src/main/java/net/bancino/log/package-info.java new file mode 100755 index 0000000..956f207 --- /dev/null +++ b/src/main/java/net/bancino/log/package-info.java @@ -0,0 +1,52 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +/** + * LogIO: A reflection-based logging library built specifically for SwerveIO. + * + *

+ * LogIO attempts to solve SwerveIO's logging situation. In previous versions + * of SwerveIO, an interface had to be implemented to log certain aspects of + * the SwerveIO runtime. However, this was clunky, because it could only data + * and objects that were accessible via the public API. This led to a number of + * variables being exposed via the public API in a way that could allow users to + * seriously abuse the way SwerveIO works. Furthermore, it made maintaining SwerveIO + * very difficult because so much of it was exposed to the public API that hardly + * any of it could be modified as needed in any meaningful way. + *

+ *

+ * LogIO solves this situation by using reflection. Although reflection has it's + * own flaws, it better suits SwerveIO because it allows SwerveIO authors to define + * how and at what level particular data should be logged without giving SwerveIO + * users the ability to modify that data in undefined ways. It does this by + * wrapping all values gathered via reflection in {@link LogEntry}. + *

+ *

+ * LogIO is a tree-based logging facility. This means that it recursively processes + * log targets and generates the log representation in such a way that a log entry may + * have any number of parent and child enries, depending on where it is in the + * hierarchy. The generated hierarchy directly mirrors the code from which the + * hierarchy is derived. LogIO is thus best suited to generating JSON logs that + * accurately represent the state of a program at a given time, although any + * situation in which tree-based logging is appropriate can utilize LogIO by implementing + * {@link LogHandler}. + *

+ * + * @author Jordan Bancino + * @version 7.0.0 + * @since 7.0.0 + */ +package net.bancino.log; diff --git a/src/main/java/net/bancino/robotics/swerveio/SwerveDrive.java b/src/main/java/net/bancino/robotics/swerveio/SwerveDrive.java new file mode 100755 index 0000000..dbbc5f2 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/SwerveDrive.java @@ -0,0 +1,1410 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio; + +import java.io.File; +import java.io.FileInputStream; +import java.io.FileOutputStream; +import java.io.IOException; +import java.util.Map; +import java.util.Properties; +import java.util.EnumMap; +import java.util.function.BiConsumer; +import java.util.function.Consumer; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; +import net.bancino.robotics.swerveio.geometry.ChassisDimension; +import net.bancino.robotics.swerveio.geometry.ModuleVector; +import net.bancino.robotics.swerveio.geometry.SwerveVector; +import net.bancino.robotics.swerveio.geometry.Length.Unit; +import net.bancino.robotics.swerveio.gyro.Gyro; +import net.bancino.robotics.swerveio.gyro.WPILibGyro; +import net.bancino.robotics.swerveio.kinematics.SwerveKinematicsProvider; +import net.bancino.robotics.swerveio.kinematics.DefaultSwerveKinematics; +import net.bancino.robotics.swerveio.log.RobotLogger; +import net.bancino.robotics.swerveio.module.SwerveModule; +import net.bancino.robotics.swerveio.pid.PIDController; +import net.bancino.robotics.swerveio.pid.DefaultPIDController; + +/** + * A class that extends a WPILib subsystem to implement a swerve drive. + * + *

+ * This takes care of consolidating swerve modules so they can easily be driven + * as a system. This class is compatible with the "new" command framework + * provided by the {@code edu.wpi.first.wpilibj2.command} package. It is + * not compatible with the original command framework, and therefore + * requires WPILib 2020 or later. + *

+ *

+ * The {@code SwerveDrive} is extremely flexible, offering multiple + * {@code drive()} methods, as you'll see below. The different methods allow + * driving the robot in different ways. For example, + * {@link #drive(SwerveVector)} would be most appropriately used for driving + * with a joystick, since it uses percentages of full output to drive the robot, + * generally in the same units as whatever the joystick outputs. On the other + * hand, {@link #drive(ChassisSpeeds)} takes an absolute speed reference, in + * meters per second and/or radians per second. This makes it ideal for + * autonomous, especially waypoint autonomous, where the state of the robot at + * each loaded as an absolute position/velocity vector. + *

+ *

+ * While there are many methods available in this class, the only methods that + * the standard SwerveIO user will use should be the {@code drive()} methods, as + * well as perhaps {@link #setReversed(DegreeOfFreedom, boolean)}, for reversing + * axes. If the programmer utilizes the command package that SwerveIO provides, + * they may not even have to touch {@code drive()} either, at least for + * tele-operated period. Autonomous may require gyro resets or other state + * setting, but SwerveIO provides an API for PathWeaver, and a command to + * automatically drive the swerve with it. + *

+ *

+ * If you need to debug your swerve drive, you can use the logging API, which is + * defined in this class, as well as in the + * {@link net.bancino.robotics.swerveio.log} package. The log package provides + * the actual implementations of the loggers, which pull state information from + * the API provided by this class and push data to the output destination. See + * the log package documentation for more details. Every swerve drive logger is + * passed a hierarchal collection of variables. If you are using an + * already-existing logging implementation, see {@link #getLogger()} to add it. + *

+ *

+ * This class automatically takes care of calling the loggers registered to it + * by overriding {@code SubsystemBase.periodic()} (see {@link #periodic()}) to + * interatively call the logging methods, so you do not have to manually add the + * swerve drive logger to your robot's iterative methods, allowing for cleaner, + * more efficient code. You can add your loggers via + * {@link SwerveDrive#getLogger()} and then not worry about them anymore. + *

+ * + * @author Jordan Bancino + * @version 6.1.0 + * @since 1.0.0 + */ +@Loggable +public class SwerveDrive extends SubsystemBase { + + /* + * The GNU GPL recommends outputting a copyright message and states that you are + * not allowed to remove this copyright message if it is present. + */ + static { + System.out.println("********************************************************************************"); + System.out.println("* SwerveIO - Copyright (C) Jordan Bancino *"); + System.out.println("* *"); + System.out.println("* This program comes with ABSOLUTELY NO WARRANTY. This is free software, and *"); + System.out.println("* you are welcome to redistribute it under certain conditions. For details, *"); + System.out.println("* see the license file that should have been distributed with this program. If *"); + System.out.println("* you did not receive a license with this program, visit *"); + System.out.println("* https://bancino.net/licenses/gpl-3.0.txt *"); + System.out.println("********************************************************************************"); + } + + /** + * Build a new swerve drive object. This class makes it easy to set up the + * swerve drive with all the components it requires to run, and is required by + * SwerveIO. In cases such as this one, SwerveIO will follow the "builder + * pattern" of Java, because often each field used in the object's construction + * requires a substantial amount of setup. The builder pattern is beyond the + * scope of this documentation, but the details of this class and how to use it + * are provided here. + * + *

+ * To construct a swerve drive, you must provide the following information: + *

+ *
    + *
  • The dimensions of your swerve drive
    • + *
  • The CAN/PWM motors to use. These should implement WPILib's + * {@code SpeedController}
    • + *
  • The drive and pivot encoders attached to the motors. These will implement + * {@link net.bancino.robotics.swerveio.encoder.Encoder}
    • + *
+ *

+ * Optionally, you can provide a gyro for field-centric navigation, or your own + * custom swerve drive kinematics provider for controlling modules. At this + * stage in the system code, you are not assigning joysticks or anything of that + * sort (unless you use the initialize block, as documented below). This builder + * is merely to bind the actual hardware of your swerve drive to the swerve + * drive object. + *

+ *

+ * Unless otherwise stated, all methods in this class overwrite the previous + * value of the field they set, so if you call the same method twice, the latest + * call will take precedence over the previous call. + *

+ *

Example

+ *

+ * Here is a basic example (with many details omitted for the sake of brevity) + * that shows how to construct a swerve drive using this builder class. + *

+ * + * 
+     * {@code
+     * import net.bancino.robotics.swerveio.SwerveDrive;
+     * import net.bancino.robotics.swerveio.geometry.ChassisDimension;
+     * import net.bancino.robotics.swerveio.geometry.Length;
+     * import net.bancino.robotics.swerveio.geometry.Unit;
+     * import net.bancino.robotics.swerveio.SwerveModule;
+     * import net.bancino.robotics.swerveio.DegreeOfFreedom;
+     *
+     * public class Drivetrain {
+     *   // In your RobotContainer class, or wherever you declare your subsystems,
+     *   // you an call Drivetrain.create() to construct your custom swerve drive.
+     *   public static SwerveDrive create() {
+     *     return new SwerveDrive.Builder()
+     *       .useDefaultKinematics(new ChassisDimension(new Length(27.4, Unit.INCHES))) // This should be the size of your robot.
+     *       .setGyro(gyro) // Create/add your gyro here.
+     *       .setModuleMap((map) -> {
+     *         map.put(SwerveModule.FRONT_RIGHT, frontRight); // Create/add your front right module here
+     *         map.put(SwerveModule.FRONT_LEFT, frontLeft);   // Create/add your front left module here
+     *         map.put(SwerveModule.REAR_LEFT, rearLeft);     // Create/add your rear left module here
+     *         map.put(SwerveMOdule.REAR_RIGHT, rearRight);   //Create/add your rear right module here
+     *       }, (module) -> {
+     *         // This code is run on every one of the modules added to the map, above.
+     *         var pid = module.getPivotPIDController();
+     *         pid.setOutputRampRate(0.1);
+     *         pid.setP(0.003);
+     *         pid.setI(0.000015);
+     *         pid.setD(0);
+     *       }).build((swerve) -> {
+     *         // Additional configuration on the swerve drive that isn't available in the builder
+     *         swerve.setReversed(DegreeOfFreedom.FORWARD, true);
+     *       });
+     *   }
+     * }
+     * }
+     *
+ * + *

+ * Note that this example is not intended to explain functions like + * {@link SwerveDrive#setReversed(DegreeOfFreedom, boolean)} or + * {@link net.bancino.robotics.swerveio.pid.PIDController#setP(double)}. Nor is + * it intended to compile. You must add your code where the comments indicate, + * namely at {@code gyro}, {@code frontRight}, {@code frontLeft}, + * {@code readLeft}, and {@code rearRight}. It should also be noted that the + * example PID values above are by no means an indication of what your values + * should be. They are merely for the sake of completeness. You can create + * swerve modules by implementing + * {@link net.bancino.robotics.swerveio.module.SwerveModule}, instantiating + * {@link net.bancino.robotics.swerveio.module.GenericSwerveModule}, or choosing + * a hardware module that is aready created and tested. Each swerve mdoule + * requires a minimum of one (1) encoder: the pivot encoder, as well as two + * motors. See the module documentation (available at the link above) for + * details on how to add encoders and motors to your modules. + *

+ *

+ * Once you have constructed a swerve drive object, you can proceed to drive + * your robot with it as documented in {@link SwerveDrive}. + *

+ * + * @see SwerveDrive + * @see net.bancino.robotics.swerveio.module.SwerveModule + */ + public static class Builder { + private SwerveKinematicsProvider kinematicsProvider = null; + private Map moduleMap = null; + + private Gyro gyro = null; + private PIDController gyroPID = null; + private int gyroStandingSlot; + private int gyroMovingSlot; + + private Runnable periodic; + + /** + * Instantiate a new SwerveDrive builder. This accepts no parameters and does + * nothing by default, because this class is a builder class: you call it's + * methods to set it up. When you're done, call {@link #build()} or + * {@link #build(Consumer)} to get the swerve drive object. Note that it still + * may require extra initialization before it is ready to be used. For example, + * you may set properties such as the "idle angle", and may want to reverse + * axes. To do this before retrieving the {@link SwerveDrive} object from this + * builder, use {@link #build(Consumer)}, providing a {@link SwerveDrive} + * consumer. + */ + public Builder() { + } + + /** + * Set the kinematics provider for this swerve drive. The kinematics provider is + * responsible for supplying the dimensions of the swerve drive, as well as + * performing all the mathematical computations for the swerve modules. If you + * aren't sure what this does, or if you don't plan on implementing your own + * kinematics provider, use {@link #useDefaultKinematics(ChassisDimension)} + * instead. This function is for programmers that want to change the fundamental + * way their swerve drive is driven. Users that just need a working swerve drive + * can get by with the default kinematics without issue. The default kinematics + * are of the highest quality and are used in competitions, so don't feel that + * you have to implement the kinematics provider interface yourself. + * + * @param kinematicsProvider The kinematics provider to use. This provides the + * dimensions for your swerve drive. + * @return The instance this method was called on so that methods can be + * chained. + * @see net.bancino.robotics.swerveio.kinematics.SwerveKinematicsProvider + */ + public Builder setKinematicsProvider(SwerveKinematicsProvider kinematicsProvider) { + this.kinematicsProvider = kinematicsProvider; + return this; + } + + /** + * Use the default swerve drive mode with your swerve drive's linear dimensions. + * Under most circumstances, this is how you will set the dimensions of your + * swerve drive. The dimensions are used in the vector calculations for each + * module, as well as computing the angular velocity of your robot as it moves. + * If you use a custom kinematics provider, see + * {@link #setKinematicsProvider(SwerveKinematicsProvider)}. Your kinematics + * provider is responsible for providing the chassis dimensions too. + * + *

+ * If you don't know what a kinematics provider is, use this method, because you + * can directly provide your chassis dimensions. + *

+ * + * @param chassis Your swerve drive's dimensions. See + * {@link net.bancino.robotics.swerveio.geometry.ChassisDimension}. + * @return The instance this method was called on so that methods can be + * chained. + * @see net.bancino.robotics.swerveio.kinematics.SwerveKinematicsProvider + */ + public Builder useDefaultKinematics(ChassisDimension chassis) { + return setKinematicsProvider(new DefaultSwerveKinematics(chassis)); + } + + /** + * Set the gyro that this swerve drive should use. If you provide a gyro, + * field-centric navigation will be enabled, and you can optionally disable it + * later. If you do not provide a gyro, field-centric navigation is disabled and + * you cannot enable it after the creation of the swerve drive. + * + * @param gyro The gyro implementation to use for field-centric navigation. + * @return The instance this method was called on so that methods can be + * chained. + * @see net.bancino.robotics.swerveio.gyro.Gyro + */ + public Builder setGyro(Gyro gyro) { + this.gyro = gyro; + return this; + } + + /** + * Set the gyro that this swerve drive should use. If you provide a gyro, + * field-centric navigation will be enabled, and you can optionally disable it + * later. If you do not provide a gyro, field-centric navigation is disabled and + * you cannot enable it after the creation of the swerve drive. + * + * @param gyro The gyro implementation to use for field-centric navigation. + * @return The instance this method was called on so that methods can be + * chained. + * @see edu.wpi.first.wpilibj.interfaces.Gyro + */ + public Builder setGyro(edu.wpi.first.wpilibj.interfaces.Gyro gyro) { + return setGyro(new WPILibGyro(gyro)); + } + + /** + * Tune the gyro PID controller to be used for angle positioning in + * {@link #drive(SwerveVector,double)}. This controller is used to generate the + * rotational parameter of a swerve drive vector. It recieves gyro angles and + * produces a percentage output on the rotational component. + * + *

+ * This method uses the default PID controller with + * {@link #setAnglePID(PIDController, int, int, Consumer)}. Refer to the + * documentation for that method for parameter descriptions. + *

+ * + * @param standingSlot See + * {@link #setAnglePID(PIDController, int, int, Consumer)}. + * @param movingSlot See + * {@link #setAnglePID(PIDController, int, int, Consumer)}. + * @param initialize See + * {@link #setAnglePID(PIDController, int, int, Consumer)}. + * @return The instance this method was called on so that methods can be + * chained. + */ + public Builder setAnglePID(int standingSlot, int movingSlot, Consumer initialize) { + return setAnglePID(new DefaultPIDController(), standingSlot, movingSlot, initialize); + } + + /** + * Set a PID controller to be used for angle positioning in + * {@link #drive(SwerveVector,double)}. This controller is used to generate the + * rotational parameter of a swerve drive vector. It recieves gyro angles and + * produces a percentage output on the rotational component. + * + * @param gyroPID A PID controller that accepts gyro angles and outputs the + * rotational component of a swerve vector. + * @param standingSlot See + * {@link #setAnglePID(PIDController, int, int, Consumer)} + * for details. + * @param movingSlot See + * {@link #setAnglePID(PIDController, int, int, Consumer)} + * for details. + * + * @return The instance this method was called on so that methods can be + * chained. + * + * @see net.bancino.robotics.swerveio.geometry.SwerveVector + */ + public Builder setAnglePID(PIDController gyroPID, int standingSlot, int movingSlot) { + return setAnglePID(gyroPID, standingSlot, movingSlot, null); + } + + /** + * Set a PID controller to be used for angle positioning in + * {@link #drive(SwerveVector,double)}. This controller is used to generate the + * rotational parameter of a swerve drive vector. It recieves gyro angles and + * produces a percentage output on the rotational component. + * + *

+ * Starting with SwerveIO 5.0.4, gyro PID controllers must have two active + * profile slots configured. See + * {@link net.bancino.robotics.swerveio.pid.PIDController} for information on + * profile slots. Gyro PID controllers must have these two slots configured + * because swerve drives behave differently depending on whether they're + * stationary or in movement. Rotation while in movement is often easier and + * requires lower PID gains than rotation while stationary. + *

+ * + * @param

The type of the PID controller you're using. + * @param gyroPID A PID controller that accepts gyro angles and outputs the + * rotational component of a swerve vector. + * @param standingSlot The PID profile slot to use when the robot is stationary. + * Typically this has higher gains to break the friction of + * the swerve drive. + * @param movingSlot The PID pofile slot to use when the robot is in motion. + * Typically this has lower gains because there is a lot + * less resitance to movement when the swerve drive is + * already moving. + * @param initialize Initialize the passed PID controller. This can be used to + * perform extra setup not provided by constructors. Note + * that the setpoint range and the maximum output are + * automatically set after this function is executed. You + * can also configure the two required profile slots in this + * function if you haven't done so before passing the PID + * controller into this method. + * @return The instance this method was called on so that methods can be + * chained. + */ + public

Builder setAnglePID(P gyroPID, int standingSlot, int movingSlot, + Consumer

initialize) { + if (initialize != null) { + initialize.accept(gyroPID); + } + this.gyroPID = gyroPID; + this.gyroStandingSlot = standingSlot; + this.gyroMovingSlot = movingSlot; + return this; + } + + /** + * Set a PID controller to be used for angle positioning in + * {@link #drive(SwerveVector,double)}. This controller is used to generate the + * rotational parameter of a swerve drive vector. It recieves gyro angles and + * produces a percentage output on the rotational component. + * + * @param gyroPID A PID controller that accepts gyro angles and outputs the + * rotational component of a swerve vector. This PID controller + * must have slots 0 and 1 configured for standing and moving + * gains respectively. See + * {@link #setAnglePID(PIDController, int, int)} for details. + * + * @return The instance this method was called on so that methods can be + * chained. + * + * @see net.bancino.robotics.swerveio.geometry.SwerveVector + */ + public Builder setAnglePID(PIDController gyroPID) { + return setAnglePID(gyroPID, null); + } + + /** + * Set a PID controller to be used for angle positioning in + * {@link #drive(SwerveVector,double)}. This controller is used to generate the + * rotational parameter of a swerve drive vector. It recieves gyro angles and + * produces a percentage output on the rotational component. + * + * @param gyroPID A PID controller that accepts gyro angles and outputs the + * rotational component of a swerve vector. This PID + * controller must have slots 0 and 1 configured for standing + * and moving gains respectively. See + * {@link #setAnglePID(PIDController, int, int, Consumer)} for + * details. + * @param initialize Initialize the passed PID controller. This can be used to + * perform extra setup not provided by constructors. Note that + * the setpoint range and the maximum output are automatically + * set after this function is executed. + * + * @return The instance this method was called on so that methods can be + * chained. + * + * @see net.bancino.robotics.swerveio.geometry.SwerveVector + * @see net.bancino.robotics.swerveio.pid.PIDController + */ + public Builder setAnglePID(PIDController gyroPID, Consumer initialize) { + return setAnglePID(gyroPID, 0, 1, initialize); + } + + /** + * Create and initialize a swerve module map. This method automatically creates + * a map object for you before passing it to the map initializer. In the + * initializer, it is your job to set up the swerve modules and add them to the + * module map. + * + *

+ * The "module map" is a Java map object consisting of exactly 4 elements: one + * for each swerve module. You may provide no more and no less than 4 modules. + * Each module in the map should be placed under it's corresponding enumeration + * value. See {@link SwerveModule.Location} for the valid enumeration values. + *

+ *

+ * While it would be standard practice to initialize your modules in the + * initialization function, it is by no means necessary. You can instantiate and + * initialize them in any way, but you will use the consumer function to consume + * an already-existing, empty map, to which you will add your four swerve + * modules using {@code Map.put()}. + *

+ * + * @param mapInitializer The initializer function that adds the swerve modules + * to the provided module map. This method does nothing if + * this parameter is null. + * @return The instance this method was called on so that methods can be + * chained. + * @see net.bancino.robotics.swerveio.module.SwerveModule + * + */ + public Builder setModuleMap(Consumer> mapInitializer) { + if (mapInitializer != null) { + Map moduleMap = new EnumMap<>(SwerveModule.Location.class); + mapInitializer.accept(moduleMap); + this.moduleMap = moduleMap; + } + return this; + } + + /** + * Set the periodic function to be run on the swerve drive's event thread. + * Because you cannot override {@link SwerveDrive#periodic()}, you can set this + * runnable to be executed inside it. + * + * @param periodic A function to execute periodically, or null to not run any + * function. + * + * @return The instance this method was called on so that methods can be + * chained. + */ + public Builder setPeriodic(Runnable periodic) { + this.periodic = periodic; + return this; + } + + /** + * Build a new swerve drive object by calling the internal constructor + * {@link SwerveDrive#SwerveDrive(Builder)} — which is inaccessible to the + * public, see {@link SwerveDrive#SwerveDrive(Builder)} for details — with + * this builder object, completing the configuration and checking it to ensure + * that it is correct. All error-checking code happens in the constructor, not + * in this builder class. + * + * @param initialize Initialize the newly created swerve drive. Use this to set + * up the swerve drive before this method returns it. + * @return Your new swerve drive. At this point, it should be fully configured + * and ready to pass into a command. + * @throws IllegalArgumentException If there is a configuration error. This is a + * runtime exception that you should not + * catch, because if there is a fatal error, it + * will stop your robot code, which is the + * intended behavior. + */ + public SwerveDrive build(Consumer initialize) throws IllegalArgumentException { + SwerveDrive swerve = new SwerveDrive(this); + if (initialize != null) { + initialize.accept(swerve); + } + return swerve; + } + + /** + * See {@link #build(Consumer)}, this is the same thing, but it passes + * {@code null} in for the consumer. Use this if you do not want to initialize + * the swerve drive object before it is returned. + * + * @return Your new swerve drive. Note that it may require additional + * configuration because by using this method instead of + * {@link #build(Consumer)}, you opt not to execute initialization code + * on it. + * @throws IllegalArgumentException If there is a configuration error. + */ + public SwerveDrive build() throws IllegalArgumentException { + return build(null); + } + } + + /** + * SwerveIO fully supports the three degrees of freedom. This enumeration lists + * them for use in methods that may require a degree of freedom be represented. + * Currently, the only method that uses this enumeration is + * {@link SwerveDrive#setReversed(DegreeOfFreedom, boolean)}, which is used to + * reverse the swerve drive input for a particular degree of freedom. + * + *

+ * A "degree of freedom" is most simply an axis of motion. Swerve drive has + * three axes of motion: forward/reverse (called forward), left/right (called + * strafe), and angular rotation (called rotation). Sometimes these are given + * labels as they would appear on a coordinate plane, where X represents the + * strafe axis, Y represents the forward axis, and Z represents the rotation + * axis. + *

+ * + * @author Jordan Bancino + * @version 3.1.0 + * @since 2.2.0 + * @see net.bancino.robotics.swerveio.geometry.SwerveVector + */ + public static enum DegreeOfFreedom { + /** + * Forward/backward motion, commonly represented on the Y axis. + */ + FORWARD, + + /** + * Right/left motion, commonly represented on the X axis. + */ + STRAFE, + + /** + * Clockwise/counterclockwise motion, commonly represented on the Z axis. + */ + ROTATION + } + + /** + * The default file that is written to and read from when using + * {@link #saveAngleOffsets()} and {@link #loadAngleOffsets()}, respectively. + */ + protected static final File DEFAULT_ANGLE_OFFSET_FILE = new File(Filesystem.getOperatingDirectory(), + "SwerveIOAngleOffsets.properties"); + + @Log(as = "modules", atLevel = LogLevel.IMPORTANT) + private final Map moduleMap; + + @Log(as = "kinematics", atLevel = LogLevel.INFO) + private final SwerveKinematicsProvider calc; + + @Log(atLevel = LogLevel.INFO) + private boolean fieldCentric = true; + + @Log(atLevel = LogLevel.INFO) + private final Gyro gyro; + + @Log(atLevel = LogLevel.DEBUG) + private final PIDController gyroPID; + private boolean gyroSlotChanged = false; + + @Log + private final int gyroStandingSlot; + + @Log + private final int gyroMovingSlot; + + /** + * When not moving, the idle angle is the default position. This is set + * initially to 135, to point the modules in towards the center of the bot. + */ + @Log + private double idleAngle = 135; + + @Log + private boolean idleAngleEnabled = false; + + /** + * Diagonal flip will alternate the idle angle on the modules, causing them to + * point opposite on diagonal corners. For instance, if you have an idle angle + * of 135, setting this to true will set two diagonal modules to 45, because it + * will flip them all so they point toward the center. + */ + @Log + private boolean diagonalFlipIdle = true; + + @Log(as = "vector", atLevel = LogLevel.IMPORTANT) + private SwerveVector lastSwerveVector = new SwerveVector(0, 0, 0); + + /* Used for idle-angle calculations. */ + private final EnumMap lastPivotAngle = new EnumMap<>(SwerveModule.Location.class); + + private final EnumMap reversedInput = new EnumMap<>(DegreeOfFreedom.class); + + private final Runnable periodic; + + @Log(as = "logger") + private final RobotLogger log = new RobotLogger(); + + /** + * Construct the actual swerve drive object. This constructor could very well be + * {@code private}, but it has been made {@code protected} to allow subclassing. + * The normal way of creating a swerve drive object would be to use the + * {@link Builder} class, but if you are extending this class, you wouldn't be + * able to call this constructor. By making it protected, you can manually + * invoke it with a builder object that you create in your class constructor. + * + * @param builder The builder object used to construct the swerve drive class. + * This object acts as a wrapper for all the required values. + * @see SwerveDrive.Builder + */ + protected SwerveDrive(Builder builder) { + if (builder == null) { + throw new IllegalArgumentException("SwerveDrive Builder cannot be null."); + } + if (builder.moduleMap != null) { + if (builder.moduleMap.size() == SwerveModule.Location.values().length) { + this.moduleMap = builder.moduleMap; + forEachModule((SwerveModule.Location location, SwerveModule module) -> { + if (module == null) { + throw new IllegalArgumentException("The " + location + " swerve module is null."); + } + }); + } else { + throw new IllegalArgumentException("Expected map of size " + SwerveModule.Location.values().length + + ". Got size " + builder.moduleMap.size()); + } + } else { + throw new IllegalArgumentException("Expected a module map, but the swerve builder didn't provide one."); + } + if (builder.kinematicsProvider != null) { + this.calc = builder.kinematicsProvider; + } else { + throw new IllegalArgumentException( + "Expected a kinematics provider, but the swerve builder didn't provide one."); + } + + /* Gyro configuration */ + this.gyro = builder.gyro; + this.gyroPID = builder.gyroPID; + + /* + * Whether or not these will be used doesn't matter, they just need to be set + * because they're final. + */ + this.gyroStandingSlot = builder.gyroStandingSlot; + this.gyroMovingSlot = builder.gyroMovingSlot; + + /* + * Configure the gyro PID controller if one was provided. + */ + if (this.gyroPID != null) { + /* Gyro goes from 0 -> 360 */ + this.gyroPID.setSetpointRange(360); + + /* Acceptable error is half a degree; a quarter degree on each side. */ + this.gyroPID.setAcceptableError(0.25); + + /* Each slot's output limit is 1. */ + this.gyroPID.setOutputLimits(this.gyroStandingSlot, 1); + this.gyroPID.setOutputLimits(this.gyroMovingSlot, 1); + } + + setFieldCentric(this.gyro != null); + + /* Set a user-defined periodic function, if any. */ + this.periodic = builder.periodic; + + /* Initial values in case these aren't changed. */ + setReversed(DegreeOfFreedom.FORWARD, false); + setReversed(DegreeOfFreedom.STRAFE, false); + setReversed(DegreeOfFreedom.ROTATION, false); + } + + /** + * Execute a task on every module associated with this swerve drive. This + * provides a safe, easy way to access the module map without actually giving + * access to the module map. This is safer than just returning the module map + * because it prevents unauthorized additions and removals. + * + * @param execute A {@code BiConsumer} that accepts a a swerve module map value + * and the actual module. + * @throws NullPointerException If the passed BiConsumer is null. + */ + public void forEachModule(BiConsumer execute) { + moduleMap.forEach(execute); + } + + /** + * Get a swerve module from the module map. This is generally safer than + * returning the swerve module map, because this prevents unauthorized additions + * and subtractions to and from it. + * + * @param module The swerve module to get. See the {@link SwerveModule.Location} + * enumeration for valid values. + * @return A swerve module implementation for the given module. + */ + public SwerveModule getModule(SwerveModule.Location module) { + return moduleMap.get(module); + } + + /** + * Reverse the input of an axis of motion. If for some reason something is + * running backwards, it can be flipped here. This will reverse the input that + * is provided to the kinematics provider to produce calcuations that are + * exactly opposite. While ideally this method isn't needed at all, it can help + * quickly fix things if the gyro is out of phase or something, or your joystick + * is reading backwards + * + * @param direction The degree of motion to select. See {@link DegreeOfFreedom} + * for the degrees of freedom that SwerveIO supports. + * @param reversed Whether or not to reverse the given degree. False indicates + * that the axis should not be fipped, and true indicates that + * it should. + */ + public void setReversed(DegreeOfFreedom direction, boolean reversed) { + reversedInput.put(direction, reversed); + } + + /** + * Get whether or not a degree of freedom is reversed. By default, all axes are + * not reversed. This will only ever return true if an axis was explicitly + * reversed by the user with {@link #setReversed(DegreeOfFreedom, boolean)}. + * + * @param direction The degree of freedom to check. + * @return Whether the given direction is being reversed before being passed + * into the kinematics provider. + */ + public boolean getReversed(DegreeOfFreedom direction) { + return reversedInput.get(direction); + } + + /** + * Set the field centric mode of the swerve drive. This requires a gyro + * implementation. Field-centric operation is described in the overview of this + * documentation. This function merely enables or disables it. + * + * @param fieldCentric Whether or not field-centric drive is enabled or + * disabled. + * @throws UnsupportedOperationException If no gyro is available to the swerve + * drive object, because a gyro is + * required for field-centric navigation. + */ + public void setFieldCentric(boolean fieldCentric) { + if (fieldCentric && gyro == null) { + throw new UnsupportedOperationException("Cannot enable field-centric mode without a gyro."); + } else { + this.fieldCentric = fieldCentric; + } + } + + /** + * Whether or not this swerve drive is field centric. By default, this is true + * if a gyro is provided, but may be false if it is explicitly disabled. If no + * gyro is provided when the swerve drive is created, this will always return + * false. + * + * @return Whether or not the gyro is being used to offset the kinematic + * calculations to provide field-centric navigation. + */ + public boolean isFieldCentric() { + return fieldCentric; + } + + /** + * Set the angle at which all swerve modules will idle at. The "idle angle" is + * defined as the default angle of each individual module when no motion is + * occuring. This feature is disabled by default, which means that the modules + * will stay in the position that they were last told to go to, which is more + * energy-efficient. However, the idle angle feature is provided with the + * intention that it can be used to "lock" the swerve modules in place, which + * can prove crucial to a defensive strategy. If the modules are locked to point + * toward the center of the robot, the robot should theoretically be immovable, + * until the driver makes an effort to move the robot. + * + * @param angle The angle (in degrees) at which to idle the modules at + * when no motion is occuring. This can be any arbitrary + * value, as it will automatically be converted to the + * correct angle. + * @param diagonalFlip Whether or not to flip two diagonal angles to the + * opposite of the provided angle. This is useful for + * "locking" the swerve drive, ensuring it doesn't move. For + * example, providing an angle of 45 and setting this + * parameter to true, you can lock the swerve in place to + * prevent it from being pushed around. If this is true, the + * front right and rear left modules are + * flipped from the other two. + */ + public void setIdleAngle(double angle, boolean diagonalFlip) { + if (angle < 0) { + this.idleAngle = (angle <= -360) ? 360 - Math.abs(angle) % 360 : 360 - Math.abs(angle); + } else { + this.idleAngle = (angle >= 360) ? angle % 360 : angle; + } + this.diagonalFlipIdle = diagonalFlip; + enableIdleAngle(true); + } + + /** + * The idle angle functionality tells the wheels to snap to a certain angle when + * the drivetrain is idling. See {@link #setIdleAngle(double, boolean)}. This is + * disabled by default, but can be toggled with this function. Note that this + * method is implied if you call {@link #setIdleAngle(double, boolean)}, so + * there is no need to call it explicitly if you are not using the default + * idle-angle. + * + * @param enabled Whether or not to enable the idle angle functionality. This be + * called at any point in the lifetime of the robot code, even + * while it is being driven. This allows it to function as a + * convenient locking mechanism that can be employed at the press + * of a button. + */ + public void enableIdleAngle(boolean enabled) { + this.idleAngleEnabled = enabled; + } + + /** + * The fastest velocity that this base can achieve, based on its slowest swerve + * module. This will iterate over the module map and calculate the max velocity + * of each module, then return the slowest velocity. + * + *

+ * Note to module developers: It is important to make sure your module's + * implementation reports an accurate maximum velocity, because it is necessary + * in the process of converting relative velocity vectors into absolute velocity + * vectors. {@link net.bancino.robotics.swerveio.geometry.SwerveVector} is + * considered a percentage vector, which means it provides a percentage + * of the maximum velocity. WPILib's {@code ChassisSpeeds} vector is absolute; + * it provides exact measurements. So, to convert from one to the other, it is + * necessary to have accurate maximum velocities, even if they are theoretical. + *

+ * + * @return The velocity of the slowest swerve module, which will be all the + * faster this swerve drive can go, in meters per second. + * + * @see net.bancino.robotics.swerveio.module.SwerveModule#getMaxSpeed() + */ + public double getChassisMaxSpeed() { + double maxSpeed = 0; + for (SwerveModule module : moduleMap.values()) { + double speed = module.getMaxSpeed(); + if (speed > maxSpeed) { + maxSpeed = speed; + } + } + return maxSpeed; + } + + /** + * The fastest angular velocity that this base can achieve, based on + * {@link #getChassisMaxSpeed()} and the dimensions of the chassis itself. + * + * @return The maximum angular velocity in radians per second that this swerve + * drive can achieve. + */ + public double getChassisMaxOmega() { + double maxRevsPerSecond = (calc.getChassisDimensions().getRadius().get(Unit.METERS) * Math.PI) + / getChassisMaxSpeed(); + return maxRevsPerSecond * (2 * Math.PI); + } + + /** + * Drive the entire chassis with a WPILib ChassisSpeeds object. This converts + * the chassis speed object into a raw swerve vector using + * {@link net.bancino.robotics.swerveio.geometry.SwerveVector#fromChassisSpeeds(ChassisSpeeds,SwerveDrive)}. + * + *

+ * This method would be the most useful for autonomous mode, as it provides an + * easy way to drive the swerve with vectors generated by WPILib utilities. + *

+ * + * @param v A WPILib ChassisSpeeds vector that represents the velocity to apply + * to this swerve base. See + * {@link net.bancino.robotics.swerveio.command.PathweaverSwerveDrive}. + */ + public void drive(ChassisSpeeds v) { + SwerveVector swerveVector = SwerveVector.fromChassisSpeeds(v, this); + drive(swerveVector); + } + + /** + * Drive the entire chassis with a + * {@link net.bancino.robotics.swerveio.geometry.SwerveVector} and a gyro angle + * setpoint. This method requires that a functional gyro and a tuned PID + * controller are passed via the builder. + * + *

+ * This method works by passing the current gyro angle and the specified target + * angle into the gyro PID controller and generating a new rotational parameter + * for the current scan. The rotational parameter provided by the swerve vector + * is discarded and replaced with the output of the gyro angle controller before + * being passed down to {@link #drive(SwerveVector)}. + *

+ * + *

+ * Many possible use cases for this exist; it can be useful in creating an + * angle-based joystick control loop that can correct for unwanted drift, or it + * can be used in an autonomous mode to ensure trajectory compass headings are + * reached. + *

+ * + * @param v The swerve vector to drive by. This used for the forward + * (Y) and strafe (X) parameters only. The rotational + * parameter (Z) is overwritten by a PID controller's + * calculations regarding the desired angle of the chassis. + * @param targetAngle The desired angle to reach on the gyro. This is passed + * into a pre-tuned gyro PID controller and is used to + * produce a new rotational parameter. It must be a value on + * the interval [0,360). + * + * @throws IllegalStateException If the gyro or gyro PID controller are null. + * @throws IllegalArgumentException If the target angle is out of the acceptable + * bounds. + */ + public void drive(SwerveVector v, double targetAngle) { + if (gyro == null) { + throw new IllegalStateException("A gyro is required to use this method. Add one with Builder.setGyro()."); + } + + if (gyroPID == null) { + throw new IllegalStateException( + "A gyro position controller is required to use this method. Add one with Builder.setAnglePID()."); + } + + if (targetAngle < 0 || targetAngle >= 360) { + throw new IllegalArgumentException("Angle setpoint out of bounds: " + targetAngle); + } + + /* + * If the swerve drive is moving, use the moving profile slot. Otherwise, use + * the standing profile slot. + */ + // double speedThreshold = 0.05; + // if (v.getFwd() > speedThreshold || v.getFwd() < -speedThreshold || v.getStr() + // > speedThreshold + // || v.getStr() < -speedThreshold || v.getRcw() > speedThreshold || v.getRcw() + // < -speedThreshold) { + // gyroPID.setActiveProfile(gyroMovingSlot); + // } else { + // gyroPID.setActiveProfile(gyroStandingSlot); + // } + + /* + * correctPivotFeedback() was originally intended only for the swerve module PID + * controller, but the gyro angle is the same problem, so we can use the same + * solution. + */ + double adjustedGyroAngle = SwerveModule.correctPivotFeedback(gyro.getAngle(), targetAngle, 360); + + double error = Math.abs(adjustedGyroAngle - targetAngle); + double errorThreshhold = 7; // Degrees + if (error > errorThreshhold) { + gyroSlotChanged = gyroPID.getActiveProfile() != gyroStandingSlot; + gyroPID.setActiveProfile(gyroStandingSlot); + } else { + gyroSlotChanged = gyroPID.getActiveProfile() != gyroMovingSlot; + gyroPID.setActiveProfile(gyroMovingSlot); + } + + /* Reset the PID controller if we switched slots */ + if (gyroSlotChanged) { + gyroPID.reset(); + } + + double rcw = gyroPID.getOutput(adjustedGyroAngle, targetAngle); + + double outputThreshhold = getModule(SwerveModule.Location.FRONT_RIGHT).getOutputThreshhold(); + /* + * If there is minimal to no forward or strafe movement and the PID output is + * less than the minimum allowed by the motor controllers, don't use the PID + * output. + * + * The first condition is applied because the problem this logic solves only + * occurs if the robot is performing a still rotation. If the robot is in + * motion, this isn't a problem. + */ + if ((Math.abs(v.getFwd()) < outputThreshhold || Math.abs(v.getStr()) < outputThreshhold) + && Math.abs(rcw) < outputThreshhold) { + v.setRcw(0); + } else { + v.setRcw(rcw); + } + + /* Pass the modified vector down to the actual drive method. */ + drive(v); + } + + /** + * Drive the entire chassis with a ChassisSpeeds object and a gyro angle + * setpoint. This method requires that a functional gyro and a tuned PID + * controller are passed via the builder. + * + *

+ * This method works by converting the ChassisSpeds into a SwerveVector and + * passing the current gyro angle and the specified target angle into the gyro + * PID controller and generating a new rotational parameter for the current + * scan. The rotational parameter provided by the swerve vector is discarded and + * replaced with the output of the gyro angle controller before being passed + * down to {@link #drive(SwerveVector,double)}. + *

+ * + *

+ * Many possible use cases for this exist; it can be useful in creating an + * angle-based joystick control loop that can correct for unwanted drift, or it + * can be used in an autonomous mode to ensure trajectory compass headings are + * reached. + *

+ * + * @param v The chassis vector to drive by. This used for the forward + * (Y) and strafe (X) parameters only. The rotational + * parameter (Z) is overwritten by a PID controller's + * calculations regarding the desired angle of the chassis. + * @param targetAngle The desired angle to reach on the gyro. This is passed + * into a pre-tuned gyro PID controller and is used to + * produce a new rotational parameter. + * + * @throws IllegalStateException if the gyro or gyro PID controller are null. + */ + public void drive(ChassisSpeeds v, double targetAngle) { + SwerveVector swerveVector = SwerveVector.fromChassisSpeeds(v, this); + drive(swerveVector, targetAngle); + } + + /** + * Drive the entire chassis with a + * {@link net.bancino.robotics.swerveio.geometry.SwerveVector}, a relative + * vector designed to be generated from a joystick. The references in this + * vector are passed directly down to the kinematics provider for conversion to + * module vectors. + * + * @param v The vector (X, Y, Z). See + * {@link net.bancino.robotics.swerveio.geometry.SwerveVector}, and + * {@link net.bancino.robotics.swerveio.command.SwerveDriveTeleop}, + * which generates these vectors. + */ + public void drive(SwerveVector v) { + /* Check for reversed input on the parameters and adjust accordingly. */ + if (reversedInput.get(DegreeOfFreedom.FORWARD)) { + v.setFwd(v.getFwd() * -1); + } + if (reversedInput.get(DegreeOfFreedom.STRAFE)) { + v.setStr(v.getStr() * -1); + } + if (reversedInput.get(DegreeOfFreedom.ROTATION)) { + v.setRcw(v.getRcw() * -1); + } + + /* Get the gyro reading, or zero if we aren't using one. */ + double gyroAngle = ((gyro != null) && fieldCentric) ? gyro.getAngle() : 0; + + forEachModule((location, module) -> { + /* Use the swerve drive calculator to calculate target speeds and angles. */ + ModuleVector moduleVector = calc.getModuleVector(location, v, gyroAngle); + double speed = moduleVector.getSpeed(); + double targetAngle = moduleVector.getAngle(); + + /* Get a reference to the module to get feedback from it. */ + SwerveModule swerveModule = getModule(location); + double currentAngle = swerveModule.getAngle(); + + if (!idleAngleEnabled) { + /* + * Set the target angle of the pivot motor to what it was before if there's no + * new output. + */ + if (speed < swerveModule.getOutputThreshhold()) { + if (lastPivotAngle.containsKey(location)) { + targetAngle = lastPivotAngle.get(location); + } + } else { + lastPivotAngle.put(location, currentAngle); + } + } else { /* Idle-Angle mode */ + /* + * If the calculated drive speed for this module is less than that module's + * minimum output threshhold then snap to the idle angle, not the calculated + * angle. Otherwise, drive. + */ + if (speed < swerveModule.getOutputThreshhold()) { + switch (location) { + case FRONT_LEFT: + case REAR_RIGHT: + targetAngle = idleAngle; + break; + case FRONT_RIGHT: + case REAR_LEFT: + targetAngle = diagonalFlipIdle ? 180 - idleAngle : idleAngle; + break; + default: + targetAngle = 0; + break; + } + } + } + + /* Compute the opposite angle before the optimization check. */ + double flippedAngle; + if (targetAngle >= 180) { + flippedAngle = targetAngle - 180; + } else if (targetAngle < 180) { + flippedAngle = targetAngle + 180; + } else { + flippedAngle = targetAngle; + } + + /* This is a small optimization to reverse the angle if it is more efficient. */ + double diff = Math.abs(targetAngle - currentAngle); + double flipDiff = Math.abs(flippedAngle - currentAngle); + if (diff >= 90 && flipDiff <= 90) { + targetAngle = flippedAngle; + speed *= -1; + } + + /* Pass the modified module vector down to the module. */ + moduleVector.setSpeed(speed).setAngle(targetAngle); + swerveModule.drive(moduleVector); + + /* Record inputs for logging purposes. */ + lastSwerveVector = v; + }); + } + + /** + * Stop this swerve drive. This will immediately stop this swerve drive by + * invoking {@link net.bancino.robotics.swerveio.module.SwerveModule#stop()} on + * all swerve modules. This takes effect until {@link #drive(SwerveVector)} is + * called again. At that point, the modules will begin movement again. + */ + public void stop() { + forEachModule((SwerveModule.Location location, SwerveModule module) -> { + module.stop(); + }); + } + + private static String makePropKey(SwerveModule.Location location) { + String asString = location.toString(); + int i; + while ((i = asString.indexOf('_')) >= 0) { + asString = asString.replace("_.", String.valueOf(Character.toUpperCase(asString.charAt(i + 1)))); + } + asString += "AngleOffset"; + return asString; + } + + /** + * Dump the current raw angle of each swerve module to the given properties + * object. This also updates the offset angle on each swerve module to be the + * dumped angle. This method is helpful when calibrating your swerve drive's + * encoders. Instead of having to manually program the offsets into your code, + * or manually copying them into a config file, you can simply call this method. + * + * @param props The properties object to dump the angle offsets to. This is + * helpful if you want to store the offsets in with the rest of + * your robot tuning parameters. + */ + public void saveAngleOffsets(Properties props) { + if (DriverStation.isDisabled()) { + forEachModule((SwerveModule.Location location, SwerveModule module) -> { + module.setAngleOffset(0); + double angle = module.getAngle(); + props.setProperty(makePropKey(location), Double.toString(angle)); + module.setAngleOffset(angle); + }); + } else { + DriverStation.reportError("Angle offsets cannot be saved while the robot is enabled.", false); + } + } + + /** + * Load raw angle offsets for each swerve module from the given properties + * object. This immediately updates the angle offset on each swerve module. This + * is helpful when initializing your swerve drive because you can directly load + * the angle offsets previously saved by {@link #saveAngleOffsets(Properties)}. + * + * @param props The properties object to load angle offsets from. This is + * helpful if you to store the offsets in with the rest of your + * robot tuning parameters. + * + * @throws NullPointerException If any angle offset property does not exist in + * the given properties object. + * @throws NumberFormatException If the value of any required offset property is + * not a valid double. + */ + public void loadAngleOffsets(Properties props) throws NullPointerException, NumberFormatException { + for (SwerveModule.Location location : SwerveModule.Location.values()) { + String rawOffset = props.getProperty(makePropKey(location)); + if (rawOffset != null) { + double offset = Double.parseDouble(rawOffset); + moduleMap.get(location).setAngleOffset(offset); + } else { + throw new NullPointerException( + "Property [" + makePropKey(location) + "] was not found in the given properties."); + } + } + } + + /** + * Dump the current raw angle of each swerve module to the given file. This also + * updates the offset angle on each swerve module to be the dumped angle. This + * method is helpful when calibrating your swerve drive's encoders. Instead of + * having to manually program the offsets into your code, or manually copying + * them into a config file, you can simply call this method. + * + * @param propFile The file to dump the angle offsets to. This file, if it + * exists, will be overwritten. The format of the file is the + * standard Java properties format. + * + * @throws IOException If there is an error opening the file for writing. + */ + public void saveAngleOffsets(File propFile) throws IOException { + Properties props = new Properties(); + saveAngleOffsets(props); + props.store(new FileOutputStream(propFile), "SwerveIO Angle Offsets"); + } + + /** + * Load raw angle offsets for each swerve module from the given file. This + * immediately updates the angle offset on each swerve module. This is helpful + * when initializing your swerve drive because you can directly load the angle + * offsets previously saved by {@link #saveAngleOffsets(File)}. + * + * @param propFile The file to load the angle offsets from. + * + * @throws NullPointerException If any angle offset property does not exist in + * the given properties object. + * @throws NumberFormatException If the value of any required offset property is + * not a valid double. + * @throws IOException If there is an error opening the file for + * reading. + */ + public void loadAngleOffsets(File propFile) throws IOException { + Properties props = new Properties(); + props.load(new FileInputStream(propFile)); + loadAngleOffsets(props); + } + + /** + * Dump the current raw angle of each swerve module to the default properties + * file, as defined by {@link #DEFAULT_ANGLE_OFFSET_FILE}. This also updates the + * offset angle on each swerve module to be the dumped angle. This method is + * helpful when calibrating your swerve drive's encoders. Instead of having to + * manually program the offsets into your code, or manually copying them into a + * config file, you can simply call this method. + * + * @throws IOException If there is an error opening the file for writing. + */ + public void saveAngleOffsets() throws IOException { + saveAngleOffsets(DEFAULT_ANGLE_OFFSET_FILE); + } + + /** + * Load raw angle offsets for each swerve module from the default properties + * file, as defined by {@link #DEFAULT_ANGLE_OFFSET_FILE}. This immediately + * updates the angle offset on each swerve module. This is helpful when + * initializing your swerve drive because you can directly load the angle + * offsets previously saved by {@link #saveAngleOffsets()}. + * + * @throws NullPointerException If any angle offset property does not exist in + * the given properties object. + * @throws NumberFormatException If the value of any required offset property is + * not a valid double. + * @throws IOException If there is an error opening the file for + * reading. + */ + public void loadAngleOffsets() throws IOException { + loadAngleOffsets(DEFAULT_ANGLE_OFFSET_FILE); + } + + /** + * The SubsystemBase periodic method is run once every scheduler routine, or + * about every 20 milliseconds. + */ + @Override + public final void periodic() { + /* Run a user-defined periodic function. */ + if (periodic != null) { + periodic.run(); + } + + /* Logging */ + try { + log.log(this); + } catch (UnsupportedOperationException e) { + DriverStation.reportWarning("Logging failed.", e.getStackTrace()); + } + } + + /** + * Get the logger associated with this class. This logger monitors this swerve + * drive and recursively logs all values, depending on the log level set. + * + * @return A logging instance. + */ + public RobotLogger getLogger() { + return log; + } + + /** + * Get the last point that was fed into this swerve drive. Contained in the + * point are the forward, strafe, and rotation variables of this vector. This + * will be exactly one scan behind what the actual received vector is is, unless + * {@link #drive(SwerveVector)} is not being called iteratively. In any case, + * this returns the last vector that was recorded by + * {@link #drive(SwerveVector)}. + * + * @return A Swerve vector. + */ + public SwerveVector getLastSwerveVector() { + return lastSwerveVector; + } + + /** + * Get the gyro object that this swerve drive was created with, if any. This can + * be used to read values off the gyro being used in this swerve drive, as well + * as to reset it if necessary, such as when the robot code starts. + * + * @return The gyro object that this swerve drive is reading from. Null if none. + */ + public Gyro getGyro() { + return gyro; + } + + /** + * Get the PID Controller used to generate the rotational parameter in + * {@link #drive(SwerveVector,double)}. This is + * + * @return The PID controller passed into this swerve drive from the builder, if + * any. It is very possible for this to be null. + */ + public PIDController getAnglePID() { + return gyroPID; + } + + /** + * Get the calculator that this swerve drive is using to compute the values it + * needs to drive the swerve modules. To get this swerve drive's chassis + * dimensions, use + * {@link net.bancino.robotics.swerveio.kinematics.SwerveKinematicsProvider#getChassisDimensions()}. + * + * @return A swerve drive calculator that is actively being used by the swerve + * drive. + */ + public SwerveKinematicsProvider getKinematicsProvider() { + return calc; + } +} diff --git a/src/main/java/net/bancino/robotics/swerveio/command/PathweaverSwerveDrive.java b/src/main/java/net/bancino/robotics/swerveio/command/PathweaverSwerveDrive.java new file mode 100755 index 0000000..d284e08 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/command/PathweaverSwerveDrive.java @@ -0,0 +1,324 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.command; + +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryUtil; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +import java.io.File; +import java.io.IOException; +import java.nio.file.Path; +import net.bancino.robotics.swerveio.SwerveDrive; +import net.bancino.robotics.swerveio.module.SwerveModule; + +/** + * Execute a PathWeaver trajectory on a SwerveIO Swerve Drive. This is intended + * to greatly simplify autonomous development, because you can use the standard + * WPILib PathWeaver to generate your trajectory and then just pass it here. + * + *

+ * PathWeaver trajectories are fully supported as of version 5.0.0. + *

+ * + *

+ * This command will execute until it is interrupted, or until the path has been + * completed. + *

+ * + * @author Jordan Bancino + * @version 6.0.0 + * @version 2.1.0 + */ +public class PathweaverSwerveDrive extends SwerveDriveCommand { + + /** + * Convert a Pathweaver state object into a ChassisSpeeds vector that can be + * passed into a swerve drive. + * + * @param pathweaverState The trajectory state to convert to the chassis speed + * object. + * @param angularVelocity The angular velocity to apply to the resultant vector. + * @return A ChassisSpeeds object that has the velocity components of the + * current state object, and the constant angular velocity component. + */ + protected static ChassisSpeeds convertToChassisSpeeds(Trajectory.State pathweaverState, double angularVelocity) { + double angle = pathweaverState.poseMeters.getRotation().getRadians(); + double velocity = pathweaverState.velocityMetersPerSecond; + double velocityX = velocity * Math.cos(angle); + double velocityY = velocity * Math.sin(angle); + + return new ChassisSpeeds(velocityX, velocityY, angularVelocity); + } + + /** + * Compute the average angular velocity between two trajectory states. + * Theoretically, we could and should take the derivative of each state to get + * the angular velocity, but it simply isn't worth the computing power. If the + * states passed to this function are close enough, you'll get an extremely + * close approximation of the angular velocity at the given point. + * + * @param start The starting trajectory state to read the rotation vector from. + * @param end The ending trajectory state to read the rotation vector from. + * + * @return The average angular velocity in radians per second that will + * accomplish the given rotation between the two states. + */ + protected static double getAngularVelocity(Trajectory.State start, Trajectory.State end) { + double r1 = end.poseMeters.getRotation().getRadians(); + double r2 = start.poseMeters.getRotation().getRadians(); + + double t1 = end.timeSeconds; + double t2 = start.timeSeconds; + + return (r1 - r2) / (t1 - t2); + } + + /** + * PathweaverSwerveDrive supports multiple path execution modes. These modes + * allow you to alter the way the Pathweaver trajectory is executed, based on + * the orientation of the robot, or the side of the field you are on. + */ + public static enum PathExecutionMode { + /** + * Run the path without modification. This will execute the raw Pathweaver + * trajectory without any altering of the {@code ChassisSpeeds} objects that it + * generates from {@link #convertToChassisSpeeds(Trajectory.State, double)}. + */ + NORMAL, + /** + * Run the path on the assumption that the robot is oriented backwards, or + * rotated 180 degrees from the front position. This will invert the forward + * parameter on the chassis speed vector. + */ + ROBOT_BACKWARDS, + /** + * Run the entire path backwards. This will invert both the forward and strafe + * parameters on the chassis speed vector. + */ + FULLY_BACKWARDS + } + + private final Trajectory trajectory; + private final PathExecutionMode executionMode; + + private Trajectory.State previousState; + private long startTime; + private double calTime = 0.75; + + private final boolean useGyro; + private final boolean doRotation; + + /** + * Create a Pathweaver swerve drive command that will execute the given + * pathweaver path on the given swerve drive. + * + * @param swerve The swerve drive to drive. + * @param pathweaverJson The pathweaver file to load, relative to the deploy + * directory. + * @param executionMode The mode to execute the path in. + * @throws IOException If there is an IO error when reading the pathweaver json + * file. + */ + public PathweaverSwerveDrive(SwerveDrive swerve, String pathweaverJson, PathExecutionMode executionMode) + throws IOException { + this(swerve, pathweaverJson, executionMode, true); + } + + /** + * Create a Pathweaver swerve drive command that will execute the given + * pathweaver path on the given swerve drive. + * + * @param swerve The swerve drive to drive. + * @param pathweaverJson The pathweaver file to load, relative to the deploy + * directory. + * @param executionMode The mode to execute the path in. + * @param doRotation Whether or not to enable the rotation component of the + * path. Setting this to false will prohibit the swerve + * drive from rotating while executing the path. + * @throws IOException If there is an IO error when reading the pathweaver json + * file. + */ + public PathweaverSwerveDrive(SwerveDrive swerve, File pathweaverJson, PathExecutionMode executionMode, + boolean doRotation) throws IOException { + super(swerve); + if (pathweaverJson != null) { + Path jsonPath = pathweaverJson.toPath(); + trajectory = TrajectoryUtil.fromPathweaverJson(jsonPath); + } else { + throw new IllegalArgumentException("Pathweaver JSON file name cannot be null."); + } + + if (executionMode != null) { + this.executionMode = executionMode; + } else { + throw new IllegalArgumentException("Execution mode cannot be null."); + } + + this.doRotation = doRotation; + this.useGyro = swerve.getGyro() != null && swerve.getAnglePID() != null; + } + + /** + * Create a Pathweaver swerve drive command that will execute the given + * pathweaver path on the given swerve drive. + * + * @param swerve The swerve drive to drive. + * @param pathweaverJson The pathweaver file to load, relative to the deploy + * directory. + * @param executionMode The mode to execute the path in. + * @param doRotation Whether or not to enable the rotation component of the + * path. Setting this to false will prohibit the swerve + * drive from rotating while executing the path. + * @throws IOException If there is an IO error when reading the pathweaver json + * file. + */ + public PathweaverSwerveDrive(SwerveDrive swerve, String pathweaverJson, PathExecutionMode executionMode, + boolean doRotation) throws IOException { + this(swerve, new File(Filesystem.getDeployDirectory(), pathweaverJson), executionMode, doRotation); + } + + /** + * Create a Pathweaver swerve drive command that will execute the given + * pathweaver path on the given swerve drive. + * + * @param swerve The swerve drive to drive. + * @param pathweaverJson The pathweaver file to load, relative to the deploy + * directory. + * @param doRotate Whether or not to enable the rotation component of the + * path. Setting this to false will prohibit the swerve + * drive from rotating while executing the path. + * @throws IOException If there is an IO error when reading the pathweaver json + * file. + */ + public PathweaverSwerveDrive(SwerveDrive swerve, String pathweaverJson, boolean doRotate) throws IOException { + this(swerve, pathweaverJson, PathExecutionMode.NORMAL, doRotate); + } + + /** + * Create a Pathweaver swerve drive that uses the normal path execution mode. + * + * @param swerve The swerve drive object to drive. + * @param pathweaverJson The pathweaver file to load, relative to the deploy + * directory. + * @throws IOException If there is an IO error when reading the pathweaver json + * file. + */ + public PathweaverSwerveDrive(SwerveDrive swerve, String pathweaverJson) throws IOException { + this(swerve, pathweaverJson, true); + } + + /** + * Set the calibration time on this path. This parameter controls how long this + * command will instruct the swerve drive modules to pivot to the forward + * position before the path is run. This is helpful for autonomous because it + * ensures the swerve modules are aligned (or roughly aligned) before attempting + * to drive. This will result in drastically more accurate paths. + * + * @param calTime The calibration time, in seconds. The swerve drive will + * not move during this time, instead the control loops will be + * used to drive the pivot motors to the starting position. After + * this time is up, the full path is run. The default calibration + * time is 0.75 seconds, which is quite generous for a well-tuned + * pivot PID loop. + */ + public void setCalibrationTime(double calTime) { + this.calTime = Math.abs(calTime); + } + + @Override + protected void initialize(SwerveDrive swerve) { + startTime = System.currentTimeMillis(); + } + + private double getTimeSeconds() { + long currentTime = System.currentTimeMillis(); + double timeSeconds = (double) (currentTime - startTime) / 1000d; + return timeSeconds - this.calTime; + } + + @Override + protected void execute(SwerveDrive swerve) { + double timeSeconds = getTimeSeconds(); + + /* + * If our current time is less than zero, we are in the calibration period. To + * calibrate, we simply set all the module angles to zero. The calibration time + * is important because it will take a few scans to actually achieve the angle. + */ + if (timeSeconds < 0) { + swerve.forEachModule((SwerveModule.Location location, SwerveModule module) -> { + module.setAngle(0); + }); + return; + } + + /* Get the current trajectory state from the trajectory. */ + Trajectory.State currentState = trajectory.sample(timeSeconds); + + /* + * Approximate the angular velocity by using the previous state we passed. It + * would not be efficient to compute the derivative to get the actual + * instantanious velocity, but this gets us close enough. + * + * This code is only used if the gyro is null, because if a gyro is present, we + * will be using a PID controller to ensure the angle is actually set right. + */ + double angularVelocity; + if (doRotation && !useGyro && previousState != null) { + angularVelocity = getAngularVelocity(previousState, currentState); + } else { + angularVelocity = 0; + } + + ChassisSpeeds swerveVector = convertToChassisSpeeds(currentState, angularVelocity); + + /* Flip the chassis speed axes based on the direction the path should go. */ + switch (executionMode) { + case NORMAL: + swerveVector.vyMetersPerSecond *= -1; + break; + case ROBOT_BACKWARDS: + swerveVector.vyMetersPerSecond *= -1; + swerveVector.vxMetersPerSecond *= -1; + break; + case FULLY_BACKWARDS: + swerveVector.vxMetersPerSecond *= -1; + break; + default: + throw new IllegalStateException("Unknown execution mode."); + } + + if (useGyro) { + double targetAngle = 0; + if (doRotation) { + targetAngle = currentState.poseMeters.getRotation().getDegrees(); + } + swerve.drive(swerveVector, targetAngle); + } else { + swerve.drive(swerveVector); + } + + previousState = currentState; + } + + @Override + public boolean isFinished() { + return trajectory.getTotalTimeSeconds() <= getTimeSeconds(); + } +} diff --git a/src/main/java/net/bancino/robotics/swerveio/command/SaveSwerveAngleOffsets.java b/src/main/java/net/bancino/robotics/swerveio/command/SaveSwerveAngleOffsets.java new file mode 100755 index 0000000..956c337 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/command/SaveSwerveAngleOffsets.java @@ -0,0 +1,72 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ + +package net.bancino.robotics.swerveio.command; + +import java.io.IOException; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import net.bancino.robotics.swerveio.SwerveDrive; + +/** + * Execute {@link net.bancino.robotics.swerveio.SwerveDrive#saveAngleOffsets()} + * on a swerve drive. This simple command is set up to properly save the angle + * offsets while the robot is disabled. You can add this command to the smart + * dashboard, or even a joystick button (not recommended) to easily calibrate + * your swerve drive. + * + *

+ * The general usage of this command is simple: align your swerve modules so + * that they are all facing perfectly straight in the forward position, then + * invoke this command to save their positions. You can then call + * {@link net.bancino.robotics.swerveio.SwerveDrive#loadAngleOffsets()} in your + * swerve drive's initialization code to load the saved offsets. + *

+ * + * @author Jordan Bancino + * @version 6.1.0 + * @since 6.1.0 + */ +public class SaveSwerveAngleOffsets extends InstantCommand { + + /** + * Construct a new offset-saving command. + * + * @param swerve The given swerve drive to save the angle offsets for. + */ + public SaveSwerveAngleOffsets(SwerveDrive swerve) { + super(() -> { + try { + swerve.saveAngleOffsets(); + DriverStation.reportWarning("Saved swerve drive angle offsets.", false); + + } catch (IOException e) { + DriverStation.reportError("Unable to save swerve angle offsets", e.getStackTrace()); + } + }, swerve); + } + + /** + * This signals to the command scheduler that this command is allowed to run + * when the robot is disabled. + */ + @Override + public boolean runsWhenDisabled() { + return true; + } +} diff --git a/src/main/java/net/bancino/robotics/swerveio/command/SwerveDriveCommand.java b/src/main/java/net/bancino/robotics/swerveio/command/SwerveDriveCommand.java new file mode 100755 index 0000000..8ce6074 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/command/SwerveDriveCommand.java @@ -0,0 +1,125 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.command; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import net.bancino.robotics.swerveio.SwerveDrive; + +/** + * A simple wrapper for the WPILib {@code CommandBase} that runs a command on a + * SwerveIO swerve drive. This class provides minimal functionality. It simply + * accepts a swerve drive in a constructor and then iteratively delivers that + * swerve drive into the abstract methods for convenience. + * + *

+ * This class was written with the intention of easing development of commands + * that operate on a swerve drive by removing boilerplate swerve drive code from + * those commands. Wherever possible, it is recommended that all commands that + * operate on a SwerveIO swerve drive extend this class. This class provides the + * following functions: + *

+ *
    + *
  • It automatically stores the swerve drive object so you don't have to and + * passes it into the command functions you need.
    • + *
  • It automatically adds the swerve drive as a subsystem requirement so you + * don't have to.
    • + *
+ * + * @author Jordan Bancino + * @version 5.0.4 + * @since 5.0.4 + */ +public abstract class SwerveDriveCommand extends CommandBase { + + private final SwerveDrive swerve; + + /** + * Construct a new swerve drive command with the given swerve drive. This + * command will operate on this swerve drive. + * + * @param swerve The swerve drive object that this command will use for + * performing swerve-related functions. + */ + protected SwerveDriveCommand(SwerveDrive swerve) { + if (swerve != null) { + this.swerve = swerve; + addRequirements(swerve); + } else { + throw new NullPointerException("Swerve drive cannot be null."); + } + } + + @Override + public final void initialize() { + super.initialize(); + initialize(swerve); + } + + @Override + public final void execute() { + super.execute(); + execute(swerve); + } + + @Override + public final void end(boolean interrupted) { + super.end(interrupted); + end(swerve, interrupted); + } + + /** + * Initialize this command. This method is called once when the command is + * initially scheduled. Use this for any state setting you need to do, such as + * disabling field-centric drive if it is enabled. + * + * @param swerve The swerve drive to initialize this command with. This is the + * same swerve drive passed into the constructor. + */ + protected abstract void initialize(SwerveDrive swerve); + + /** + * The main body of the command. This method is called iteratively while this + * command is scheduled. You should put your swerve driving code here. The idea + * is that eventually you'll call a {@code drive()} method on the passed swerve + * drive, but swerve drive commands can also be state-setting commands. + * + * @param swerve The swerve drive to execute this command with. This is the same + * swerve drive passed into the constructor. + */ + protected abstract void execute(SwerveDrive swerve); + + /** + * The action to take when the command ends. Called when either the command + * finishes normally, or when it is interrupted/canceled. + * + * @param swerve The swerve drive that was passed into the constructor. + * @param interrupted Whether the command was interrupted/canceled + */ + protected void end(SwerveDrive swerve, boolean interrupted) { + /* + * It isn't common to implement this, so provide a default implementation that + * does nothing. + */ + } + + /* + * Overriding this method to make it abstract requires subclasses to implement + * it, which will help prevent undesired behavior. + */ + @Override + public abstract boolean isFinished(); +} diff --git a/src/main/java/net/bancino/robotics/swerveio/command/SwerveDriveTeleop.java b/src/main/java/net/bancino/robotics/swerveio/command/SwerveDriveTeleop.java new file mode 100755 index 0000000..1a7e6fa --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/command/SwerveDriveTeleop.java @@ -0,0 +1,200 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.command; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; + +import net.bancino.robotics.swerveio.SwerveDrive; +import net.bancino.robotics.swerveio.geometry.SwerveVector; + +/** + * A simple command for driving a swerve drive with any WPILib-supported + * joystick. + * + *

+ * This command also demonstrates how simple it is to drive a SwerveIO swerve + * drive with a joystick. It simply extends {@link SwerveDriveTeleopCommand}, + * which provides support for many useful features. + *

+ * + *

+ * This command runs until it is interrupted; there is no condition under which + * this command will stop on it's own. The command scheduler should resume it + * after autonomous commands are executed. + *

+ * + *

+ * As of SwerveIO 5.0.2, this joystick command uses the gyro for angular + * movement if it is available and an angle PID controller is set up. This will + * prevent any drifting that may occur because a position loop will be running + * as the rotational parameter. See + * {@link net.bancino.robotics.swerveio.SwerveDrive#drive(SwerveVector, double)} + * and + * {@link net.bancino.robotics.swerveio.SwerveDrive.Builder#setAnglePID(PIDController,int,int,Consumer)} + * for more information on this. + *

+ * + * @author Jordan Bancino + * @version 6.1.0 + * @since 2.0.0 + */ +public final class SwerveDriveTeleop extends SwerveDriveTeleopCommand { + + private final boolean useGyro; + private double angleSetpoint; + private double degreeIncrement = 8; + + /** + * Create a new teleop command using a generic joystick and the default + * {@link edu.wpi.first.wpilibj.Joystick.AxisType} axes. The defaults are + * {@code kY} for forward, {@code kX} for strafe, and {@code kZ} for rotation. + * + *

+ * See + * {@link SwerveDriveTeleop#SwerveDriveTeleop(SwerveDrive, Joystick, Joystick.AxisType, Joystick.AxisType, Joystick.AxisType)} + * if you want to use custom axes. + *

+ * + * @param swerve The swerve drive to drive. + * @param joystick The joystick to read values from. + */ + public SwerveDriveTeleop(SwerveDrive swerve, Joystick joystick) { + this(swerve, joystick, Joystick.AxisType.kY, Joystick.AxisType.kX, Joystick.AxisType.kZ); + } + + /** + * Create a new teleop command using an Xbox controller and the default + * {@link edu.wpi.first.wpilibj.XboxController.Axis} axes. The defaults are + * {@code kLeftY} for forward, {@code kLeftX} for strafe, and {@code kRightX} + * for rotation. + * + *

+ * See + * {@link SwerveDriveTeleop#SwerveDriveTeleop(SwerveDrive, XboxController, XboxController.Axis, XboxController.Axis, XboxController.Axis)} + * if you want to use custom axes. + *

+ * + * @param swerve The swerve drive to drive. + * @param joystick The Xbox controller to read values from. + */ + public SwerveDriveTeleop(SwerveDrive swerve, XboxController joystick) { + this(swerve, joystick, XboxController.Axis.kLeftY, XboxController.Axis.kLeftX, XboxController.Axis.kRightX); + } + + /** + * Create a new teleop command using an Xbox controller and custom axes. + * + * @param swerve The swerve drive to drive. + * @param joystick The Xbox controller to read from. + * @param fwdAxis The axis that controls the foward parameter. + * @param strAxis The axis that controls the strafe parameter. + * @param rcwAxis The axis that controls the rotation parameter. + */ + public SwerveDriveTeleop(SwerveDrive swerve, XboxController joystick, XboxController.Axis fwdAxis, + XboxController.Axis strAxis, XboxController.Axis rcwAxis) { + this(swerve, joystick, fwdAxis.value, strAxis.value, rcwAxis.value); + } + + /** + * Create a new teleop command using a generic joystick and custom axes. + * + * @param swerve The swerve drive to drive. + * @param joystick The joystick to read from. + * @param fwdAxis The axis that controls the foward parameter. + * @param strAxis The axis that controls the strafe parameter. + * @param rcwAxis The axis that controls the rotation parameter. + */ + public SwerveDriveTeleop(SwerveDrive swerve, Joystick joystick, Joystick.AxisType fwdAxis, + Joystick.AxisType strAxis, Joystick.AxisType rcwAxis) { + this(swerve, joystick, fwdAxis.value, strAxis.value, rcwAxis.value); + } + + /** + * Create a new teleop command using any generic human interaction device and + * custom axes. + * + * @param swerve The swerve drive to drive. + * @param joystick The Xbox controller to read from. + * @param fwdAxis The axis that controls the foward parameter. + * @param strAxis The axis that controls the strafe parameter. + * @param rcwAxis The axis that controls the rotation parameter. + */ + public SwerveDriveTeleop(SwerveDrive swerve, GenericHID joystick, int fwdAxis, int strAxis, int rcwAxis) { + super(swerve, joystick, fwdAxis, strAxis, rcwAxis); + this.useGyro = swerve.getGyro() != null && swerve.getAnglePID() != null; + } + + /** + * Set the degree increment that the joystick modifies. This is the maximum + * number of degrees that the angle setpoint will change per scan if the + * joystick is at 100% rotation. Note that this is applied after the joystick + * ramp rate. + * + *

+ * Note: This is only applicable if the swerve drive angle PID controller + * is enabled. This increments the angle setpoint, but if there is no PID + * controller, the angle setpoint is not used. + *

+ * + * @param degreeIncrement The number of degrees, per scan, to change by when the + * robot joystick's rotational axis is at 100%. If the + * rotation is less than this, the increment actual will + * be proportional. Lower values mean the swerve drive + * will be less responsive to rotation, whereas higher + * values will result in a touchier robot. The default + * value is 8 degrees. + */ + public void setAngleIncrement(double degreeIncrement) { + this.degreeIncrement = Math.abs(degreeIncrement); + } + + @Override + protected void initialize(SwerveDrive swerve) { + /* + * Set the angle setpoint to the current gyro angle so this command doesn't move + * the robot upon starting. + */ + if (useGyro) { + angleSetpoint = swerve.getGyro().getAngle(); + } + } + + @Override + protected void execute(SwerveDrive swerve, SwerveVector joystickVector) { + if (useGyro) { + /* Modify the angle setpoint based on the rotational joystick parameter. */ + angleSetpoint += joystickVector.getRcw() * this.degreeIncrement; + angleSetpoint %= 360; + if (angleSetpoint < 0) { + angleSetpoint += 360; + } + + /* Drive the swerve drive with the angle setpoint. */ + swerve.drive(joystickVector, angleSetpoint); + } else { + /* Simply drive the swerve drive with the joystick vector */ + swerve.drive(joystickVector); + } + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/net/bancino/robotics/swerveio/command/SwerveDriveTeleopCommand.java b/src/main/java/net/bancino/robotics/swerveio/command/SwerveDriveTeleopCommand.java new file mode 100755 index 0000000..daecbd3 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/command/SwerveDriveTeleopCommand.java @@ -0,0 +1,268 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.command; + +import edu.wpi.first.wpilibj.GenericHID; + +import net.bancino.robotics.swerveio.SwerveDrive; +import net.bancino.robotics.swerveio.geometry.SwerveVector; + +/** + * A simple abstract class for writing swerve drive teleop commands. This class + * eliminates all the boilerplate code involved with writing a joystick command + * but still allows you to utilize and modify joysick input as needed before + * passing it to the swerve drive. This class is useful for writing commands + * that drive a swerve drive based on input from a joystick. It automatically + * takes care of throttling and deadbanding the joystick, as well as generating + * a swerve vector, so you can directly pass the vector down to the swerve + * drive, or perform modifications based on sensor readings, such as a gyro or + * or Limelight. + * + *

+ * The options for setting the deadband around zero, as well as the throttle, + * can be great for training users. + *

+ * + *

+ * When writing commands for a SwerveIO swerve drive that involve user joystick + * input, it is highly recommended to use this class, because it automatically + * handles many useful features and allows you to inherit the axes, throttle, + * and deadband used in another joystick command, allowing for a seemless + * transition across commands that the joystick operator won't even notice. + *

+ * + * @author Jordan Bancino + * @version 5.0.4 + * @since 5.0.4 + */ +public abstract class SwerveDriveTeleopCommand extends SwerveDriveCommand { + + private final GenericHID joystick; + private final int fwdAxis, strAxis, rcwAxis; + + private double deadband, throttle, rampRate; + + private SwerveVector lastVector = new SwerveVector(0, 0, 0); + + /** + * Construt a new swerve drive teleop command that inherits all the properties + * of an already-existing joystick command. This allows you to configure your + * joystick once with one command, then have all the other ones automatically + * inherit the settings set on that "master" command. Most commonly, you'll use + * this constructor to inherit a {@link SwerveDriveTeleop} command. + * + * @param swerve The swerve drive to pass to the superclass constructor and the + * abstract execute method. This is the swerve drive your + * joystick command will drive. + * @param inherit The swerve drive teleop command to inherit the joystick, + * joystick axes, deadband, and throttle from. The joystick is + * inherited by-reference, but the axes, deadband, and throttle + * are inherited by-value. This means that modifying the joystick + * in another command will cause it to be modified in this + * command too, but changing the deadband or throttle on another + * command will not affect this command. + */ + protected SwerveDriveTeleopCommand(SwerveDrive swerve, SwerveDriveTeleopCommand inherit) { + super(swerve); + + this.joystick = inherit.joystick; + this.fwdAxis = inherit.fwdAxis; + this.strAxis = inherit.strAxis; + this.rcwAxis = inherit.rcwAxis; + + this.deadband = inherit.deadband; + this.throttle = inherit.throttle; + this.rampRate = inherit.rampRate; + } + + /** + * Create a new teleop command using any generic human interaction device and + * custom axes. This sets a default throttle of 90%, a default deadband of 20%, + * and a default ramp rate of 0.25% per scan. See the relevant documentation for + * throttle and deadband to override these defaults. + * + * @param swerve The swerve drive to pass to the superclass constructor and + * the abstract execute method. This is the swerve drive your + * joystick command will drive. + * @param joystick The joystick to read from. + * @param fwdAxis The axis to read from that will provide values for the Y + * movement of the swerve drive. + * @param strAxis The axis to read from that will provide values for the X + * movement of the swerve drive. + * @param rcwAxis The axis to read from that will provide values for the + * angular movement of the swerve drive. + */ + protected SwerveDriveTeleopCommand(SwerveDrive swerve, GenericHID joystick, int fwdAxis, int strAxis, int rcwAxis) { + super(swerve); + if (joystick != null) { + this.joystick = joystick; + this.fwdAxis = fwdAxis; + this.strAxis = strAxis; + this.rcwAxis = rcwAxis; + } else { + throw new IllegalArgumentException("Xbox controller cannot be null."); + } + + setDeadband(0.1); + setThrottle(0.9); + setRampRate(0.025); + } + + @Override + public final void execute(SwerveDrive swerve) { + /* Grab the joystick parameters */ + double fwd = -throttle(deadband(joystick.getRawAxis(fwdAxis))); + double str = throttle(deadband(joystick.getRawAxis(strAxis))); + double rcw = throttle(deadband(joystick.getRawAxis(rcwAxis))); + + /* Create a swerve vector and ramp it. */ + SwerveVector joystickVector = ramp(new SwerveVector(fwd, str, rcw)); + + /* We have to duplicate the vector because it may be modified down the road. */ + this.lastVector = new SwerveVector(joystickVector); + + /* Pass the vector to the abstract execute method. */ + execute(swerve, joystickVector); + } + + /** + * Set a deadband on the joystick. This is a little bit of range around the zero + * mark that does absolutely nothing. This is helpful for joysticks that are + * overly sensitive or don't always read zero in the neutral position. + * + * @param deadband The deadband to set, between 0 and 1. + */ + public final void setDeadband(double deadband) { + this.deadband = deadband; + } + + /** + * Set a throttle on the joystick. This is helpful for limiting the top speed of + * the swerve drive, which can be useful for training. + * + * @param throttle The throttle to set. This is the maximum speed the joystick + * should output. So, to throttle this command at 50% power, + * you'd put in 0.5 for the throttle. + */ + public final void setThrottle(double throttle) { + this.throttle = throttle; + } + + /** + * Set a ramp rate on the joystick. This is helpful for preventing sharp changes + * in speed or direction. + * + * @param rampRate The ramp rate to set. This is the maximum amount that the + * joystick vector is allowed to change by per scan. A value of + * zero or less disables the ramp rate. + */ + public final void setRampRate(double rampRate) { + this.rampRate = rampRate; + } + + /** + * Throttle a raw value based on the currently set throttle. + * + * @param raw The raw joystick value to scale. + * @return A scaled joystick value. + */ + private final double throttle(double raw) { + return raw * throttle; + } + + /** + * Calculate a deadband + * + * @param raw The input on the joystick to mod + * @return The result of the mod. + */ + private final double deadband(double raw) { + /* Compute the deadband mod */ + if (raw < 0.0d) { + if (raw <= -deadband) { + return (raw + deadband) / (1 - deadband); + } else { + return 0.0d; + } + } else { + if (raw >= deadband) { + return (raw - deadband) / (1 - deadband); + } else { + return 0.0d; + } + } + } + + /** + * Calculate a ramp rate, using the history vector. + * + * @param v The vector to ramp. All parameters are ramped the same. + * @return The same vector, modified with the ramp rate. + */ + private final SwerveVector ramp(SwerveVector v) { + if (this.rampRate > 0) { + double maxFwd = this.lastVector.getFwd() + this.rampRate; + double minFwd = this.lastVector.getFwd() - this.rampRate; + + if (v.getFwd() > maxFwd) { + v.setFwd(maxFwd); + } + + if (v.getFwd() < minFwd) { + v.setFwd(minFwd); + } + + double maxStr = this.lastVector.getStr() + this.rampRate; + double minStr = this.lastVector.getStr() - this.rampRate; + + if (v.getStr() > maxStr) { + v.setStr(maxStr); + } + + if (v.getStr() < minStr) { + v.setStr(minStr); + } + + double maxRcw = this.lastVector.getRcw() + this.rampRate; + double minRcw = this.lastVector.getRcw() - this.rampRate; + + if (v.getRcw() > maxRcw) { + v.setRcw(maxRcw); + } + + if (v.getRcw() < minRcw) { + v.setRcw(minRcw); + } + } + return v; + } + + /** + * The main body of the command. This method is called iteratively while this + * command is scheduled. You should put your swerve driving code here. The idea + * is that you'll perform any modifications necessary on the vector, then + * eventually call a {@code drive()} method on the pasted swerve drive with the + * modified vector. + * + * @param swerve The swerve drive to execute this command with. This is + * the same swerve drive passed into the constructor. + * @param joystickVector A swerve drive vector from the joystick. This vector + * has been throttled and deadbanded according to the + * settings before it is passed here. + */ + protected abstract void execute(SwerveDrive swerve, SwerveVector joystickVector); +} diff --git a/src/main/java/net/bancino/robotics/swerveio/command/package-info.java b/src/main/java/net/bancino/robotics/swerveio/command/package-info.java new file mode 100755 index 0000000..788f37e --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/command/package-info.java @@ -0,0 +1,32 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +/** + * WPILib "new" commands for driving a SwerveIO Swerve Drive. These commands + * are designed to be customizable, and demonstrate the usage of SwerveIO + * in autonomous and user-operated modes. They are intended to be + * competition-ready and to complete the SwerveIO API, allowing users to go + * from just a collection of hardware to full joystick and autonomous control + * without any external tools. SwerveIO is fully self-contained in this + * regard; it is complete because these commands allow you to set up your + * swerve drive without writing much code. The only code you have to write is + * the configuration of the hardware and of these commands. + * + * @author Jordan Bancino + * @version 5.0.4 + * @since 2.0.3 + */ +package net.bancino.robotics.swerveio.command; \ No newline at end of file diff --git a/src/main/java/net/bancino/robotics/swerveio/encoder/AnalogEncoder.java b/src/main/java/net/bancino/robotics/swerveio/encoder/AnalogEncoder.java new file mode 100755 index 0000000..867e2ab --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/encoder/AnalogEncoder.java @@ -0,0 +1,70 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.encoder; + +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.RobotController; +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; + +/** + * An analog encoder that connects to the RoboRIO. This class can be used to + * access 5-volt encoders that connect to the analog IO ports on the RoboRIO. + * + *

+ * This is a wrapper for WPILib's {@code AnalogInput}, which reads a 12-bit + * number representing the voltage (that is, 0-5 volts). + *

+ * + * @author Jordan Bancino + * @version 5.0.0 + * @since 1.0.0 + */ +@Loggable +public class AnalogEncoder implements Encoder { + + private AnalogInput encoder; + + @Log(as = "rawValue", atLevel = LogLevel.IMPORTANT) + private double lastValue; + + /** + * Create an analog encoder. + * + * @param port The analog port on the roboRIO that this encoder is connected to. + */ + public AnalogEncoder(int port) { + encoder = new AnalogInput(port); + } + + @Override + public double get() { + lastValue = encoder.getVoltage(); + return lastValue; + } + + @Override + public double countsPerRevolution() { + /* + * The "full" voltage may not always be the same due to load on the battery and + * other factors, so instead of hard-coding "5", we pull the actual voltage on + * the 5v rail. + */ + return RobotController.getVoltage5V(); + } +} \ No newline at end of file diff --git a/src/main/java/net/bancino/robotics/swerveio/encoder/Encoder.java b/src/main/java/net/bancino/robotics/swerveio/encoder/Encoder.java new file mode 100755 index 0000000..cdafaeb --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/encoder/Encoder.java @@ -0,0 +1,59 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.encoder; + +/** + * The encoder representation used by SwerveIO. This is a complete definition of + * the encoder API. Every encoder that is used with SwerveIO must implement this + * interface. + * + *

+ * The interface itself is fairly simplistic because there are only a few + * methods and they can usually be implemented in a just a few lines of code. + *

+ * + * @author Jordan Bancino + * @version 5.0.0 + * @since 1.0.0 + */ +public interface Encoder { + + /** + * Get the encoder's current position. Note that this may not always have double + * precision. It could be an integer as well. There is no limit on the value of + * the encoder, as long as it fits within the {@code double} data type. + * + * @return The encoder's current position, directly from the hardware. This + * value should not be modified at all; it is a raw reading specified in + * arbitrary units representing an arbitrary position. + */ + public double get(); + + /** + * Get the number of counts it takes this encoder to do one full revolution. + * This is a number of arbitrary units that represents how many units it takes + * to travel one full revolution of the encoder shaft. + * + * @return How many counts it takes to perform one full revolution of the + * encoder shaft, in the units of {@link #get()}. This should + * always come from the encoder documentation; don't use an experimental + * value. Also remember that this revolutions of the encoder, not + * revolutions of the pivot mechanism, though these will be the same if + * the encoder is 1:1. + */ + public double countsPerRevolution(); +} \ No newline at end of file diff --git a/src/main/java/net/bancino/robotics/swerveio/encoder/package-info.java b/src/main/java/net/bancino/robotics/swerveio/encoder/package-info.java new file mode 100755 index 0000000..fed107a --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/encoder/package-info.java @@ -0,0 +1,63 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +/** + * Encoder iterfaces and helper classes. See {@link Encoder} for details on how + * an encoder is expected to behave. + * + *

+ * Beginning users of SwerveIO will probably want to use an existing encoder + * implementation, but they can also easily create their own implementations by + * directly implementing {@link Encoder}. + *

+ *

+ * The following encoders are officially supported by SwerveIO Core: + *

+ * + * + * + * + * + * + * + * + * + * + *
Supported Hardare Table
EncoderSwerveIO Class
Any 5-volt analog encoder that plugs into the RoboRIO analog IO port{@link AnalogEncoder}
+ *

+ * Additional encoders are supported by the official hardware vendor support + * libraries. Please refer to the documentation home page for adding hardware + * support for your encoder. + *

+ *

+ * If your encoder is listed as supported, either in the list above or on the + * home page, use the hardware-specific classes to access them. The + * documentation for each individual encoder will provide you with all the + * specifics. If your encoder is not officially supported by SwerveIO, you will + * have to do a little more work to get SwerveIO to play nice with it, but due + * to the nature of SwerveIO and the design principles it was built with, adding + * your own encoder implementation should be trivial. + *

+ *

+ * Using the interfaces and classes in this package, you can add support for any + * encoder. + *

+ * + * @author Jordan Bancino + * @version 6.0.0 + * @since 1.0.0 + */ +package net.bancino.robotics.swerveio.encoder; \ No newline at end of file diff --git a/src/main/java/net/bancino/robotics/swerveio/geometry/ChassisDimension.java b/src/main/java/net/bancino/robotics/swerveio/geometry/ChassisDimension.java new file mode 100755 index 0000000..a5cda03 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/geometry/ChassisDimension.java @@ -0,0 +1,149 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.geometry; + +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; +import net.bancino.robotics.swerveio.geometry.Length.Unit; + +/** + * An abstract representation of a physical swerve drive chassis. Objects of this class + * store the dimensions of the SwerveIO swerve base and provide methods for calculating + * the diagonal ({@link #getDiagonal()}), as well as the radius ({@link #getRadius()}). + * See the {@link #getRadius()} documentation for what the radius is. + * + *

+ * Note that the chassis dimensions are measured from module to module, not + * necessarily from physical end to physical end. A chassis may have physical dimensions + * of 30 inches, but this class should represent the measurement of center of the module + * wheel in one corner to the center of the module wheel in each adjacent corner, which + * will most likely be less; say, 24.3 inches, for example. + *

+ * + *

+ * The chassis dimensions should be accurate because they are used in the computation of + * the swerve module vectors, as well as in the computation of the angular velocity of + * the swerve drive itself. Innacurate chassis dimensions will result in undefined and + * most likely undesirable behavior. + *

+ * + * @author Jordan Bancino + * @version 5.0.0 + * @since 2.0.0 + */ +@Loggable +public class ChassisDimension { + + @Log(atLevel = LogLevel.IMPORTANT) + private final Length trackWidth, trackLength; + + /** + * Construct a chassis dimension with two lengths. This should be used for rectangular + * chassis that have different side lengths, even if only slightly. + * + * @param trackWidth The track width of the chassis, measured from left to + * right. + * @param trackLength The track length of the chassis, measured from front to + * back. + */ + public ChassisDimension(Length trackWidth, Length trackLength) { + if (trackWidth != null) { + this.trackWidth = trackWidth; + } else { + throw new IllegalArgumentException("A chassis must have a track width."); + } + if (trackLength != null) { + this.trackLength = trackLength; + } else { + throw new IllegalArgumentException("A chassis must have a track length."); + } + } + + /** + * Construct a square chassis. This can be used for a chassis that is confirmed to + * be a perfect square. + * + * @param length The length of both the track width and length. + */ + public ChassisDimension(Length length) { + this(length, length); + } + + /** + * Get the track width of this chassis. + * + * @return The width, measured from left to right. + */ + public Length getWidth() { + return trackWidth; + } + + /** + * Get the track length of this chassis. + * + * @return the length, measured from back to front. + */ + public Length getLength() { + return trackLength; + } + + /** + * Get the radius of the circle that this base will produce when rotating. + * This value is used in calculating the maximum angular velocity of a swerve drive. + * This is the same as {@code {@link #getDiagonal()} / 2}. + * + * @return The radius, that is, the length from the center of the chassis to one + * of the corners. This will work even if the base isn't a square. + */ + public Length getRadius() { + double halfLength = trackLength.get(Unit.BASE_UNIT) / 2; + double halfWidth = trackWidth.get(Unit.BASE_UNIT) / 2; + double hypotenuse = Math.sqrt(Math.pow(halfLength, 2) + Math.pow(halfWidth, 2)); + return new Length(hypotenuse, Unit.BASE_UNIT); + } + + /** + * Get the diagonal length that this base has, measured from opposite corners + * of the base. This value is used in the kinematics calculations. + * + * @return The diagonal length, that is, the length from one corner of the + * chassis to the diagonal corner. This will work even if the base isn't + * a square. + */ + public Length getDiagonal() { + double length = trackLength.get(Unit.BASE_UNIT); + double width = trackWidth.get(Unit.BASE_UNIT); + double hypotenuse = Math.sqrt(Math.pow(length, 2) + Math.pow(width, 2)); + return new Length(hypotenuse, Unit.BASE_UNIT); + } + + @Override + public boolean equals(Object o) { + if (o instanceof ChassisDimension) { + ChassisDimension cd = (ChassisDimension) o; + return cd.getLength().equals(getLength()) && cd.getWidth().equals(getWidth()); + } else { + return false; + } + } + + @Override + public String toString() { + return getLength() + " x " + getWidth(); + } +} \ No newline at end of file diff --git a/src/main/java/net/bancino/robotics/swerveio/geometry/Length.java b/src/main/java/net/bancino/robotics/swerveio/geometry/Length.java new file mode 100755 index 0000000..20e36c4 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/geometry/Length.java @@ -0,0 +1,200 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.geometry; + +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; + +/** + * An abstract length that can be retrieved in any units. This class makes use + * of {@link Unit}, which was designed specifically to provide units of length + * for use with this class. {@code Length} exists to provide an easy mechanism + * for allowing users to specify physical lengths in any units they want, + * because it allows the length to be retrieved in any units required, no matter + * what units it ws instantiated with. For example, if swerve module + * implementation specifies that the diameter of a wheel is 4 inches, the module + * method that calculates the linear velocity in meters per second can easily + * get the wheel diameter in meters without having to do any extra work. + * + *

+ * Length objects are immutable, but can be operated in very clean an efficient + * ways. + *

+ * + * @author Jordan Bancino + * @version 2.1.0 + * @since 1.3.0 + */ +@Loggable +public class Length { + + /** + * A representation of a unit with conversion factors. This primarily intended + * for use with {@link Length}, which represents arbitrary-unit lengths. This + * class really makes no sense outside of {@link Length}. Units represented here + * are sensible; this class is not intended to represent all the possible units. + * Units are also based in terms of centimeters, so the conversion factor is + * always in terms of centimeters. This makes conversions between units simple. + * + * @author Jordan Bancino + * @version 1.3.0 + * @since 1.3.0 + */ + public static enum Unit { + /** + * US Inches — Approximately 2.54 Centimeters. + */ + INCHES(2.54), + + /** + * US Feet — Approximately 30.48 Centimeters + */ + FEET(30.48), + + /** + * Metric Centimeters, the base unit of this class. + */ + CENTIMETERS(1), + + /** + * Metric Meters — Exactly 100 Centimeters. + */ + METERS(100); + + /** + * An alias to the base unit, which is centimeters. Programs should use this + * instead of directly referencing centimeters to preserve compatibility with + * new versions. I don't know that this will ever change, but if it does, + * programs that use this will still work as intended. + */ + public static final Unit BASE_UNIT = CENTIMETERS; + + private double conversionFactor; + + /** + * Construct a new unit with the given conversion factor. + * + * @param conversionFactor How many centimeters make up this unit. + */ + private Unit(double conversionFactor) { + this.conversionFactor = conversionFactor; + } + + /** + * Get the conversion factor of this unit. This is how many centimeters are + * equivalent to this unit. + * + * @return The number of centimeters in this unit. + */ + public double conversionFactor() { + return conversionFactor; + } + } + + @Log(as = "unit", atLevel = LogLevel.IMPORTANT) + private final Unit baseUnit; + + @Log(as = "value", atLevel = LogLevel.IMPORTANT) + private final double baseValue; + + /** + * Create an immutable length object with a given length and unit measure. 1 + * foot could be specified as {@code new Length(1, Unit.FEET)}, or as + * {@code new Length(12, Unit.INCHES)}. Both represent the same value, and + * demonstrate proper usage of this constructor. + * + * @param value The numerical value of this length in terms of the second + * parameter, {@code unit}. + * @param unit The unit that the value is in. + * + * @throws IllegalArgumentException If the length is negative. Lengths of zero + * are allowed. + */ + public Length(double value, Unit unit) { + if (value >= 0) { + this.baseValue = value; + this.baseUnit = unit; + } else { + throw new IllegalArgumentException("Lengths cannot be negative."); + } + } + + /** + * Get this length in the specifed units. + * + * @param unit The unit to retrieve the length in. + * @return The value of the length in the specified units. + */ + public double get(Unit unit) { + return (baseValue * baseUnit.conversionFactor()) / unit.conversionFactor(); + } + + /** + * Add two lengths. + * + * @param length The length to add. + * @return The resulting length from the operation. + */ + public Length plus(Length length) { + return new Length(baseValue + length.get(baseUnit), baseUnit); + } + + /** + * Subtract two lengths. + * + * @param length The length to subract. + * @return The resulting length from the operation. + */ + public Length minus(Length length) { + return new Length(baseValue - length.get(baseUnit), baseUnit); + } + + /** + * Multiply two lengths. + * + * @param length The length to multiply. + * @return The resulting length from the operation. + */ + public Length times(Length length) { + return new Length(baseValue * length.get(baseUnit), baseUnit); + } + + /** + * Divide two lengths. + * + * @param length The length to divide. + * @return The resulting length from the operation. + */ + public Length dividedBy(Length length) { + return new Length(baseValue / length.get(baseUnit), baseUnit); + } + + @Override + public boolean equals(Object o) { + if (o instanceof Length) { + return ((Length) o).get(Unit.BASE_UNIT) == get(Unit.BASE_UNIT); + } else { + return false; + } + } + + @Override + public String toString() { + return String.format("%.2d %s", baseValue, baseUnit.toString().toLowerCase()); + } +} \ No newline at end of file diff --git a/src/main/java/net/bancino/robotics/swerveio/geometry/ModuleVector.java b/src/main/java/net/bancino/robotics/swerveio/geometry/ModuleVector.java new file mode 100755 index 0000000..da5b92b --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/geometry/ModuleVector.java @@ -0,0 +1,214 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.geometry; + +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; + +/** + * A mutable container for a module vector that holds the module angle and speed + * for a single iteration. This is an important core class for driving a + * SwerveIO swerve drive. it is required to drive any swerve drive because, + * through the kinematics API, all {@link SwerveVector}s are converted to + * individual module vectors. This class represents a module vector; in + * other words, it describes the motion of a single swerve module only. + * + *

+ * The module vector is intended to be used only by SwerveIO, because SwerveIO + * aims to define a complete API that abstracts everything behind just a few + * WPILib commands and a subsystem. There should be no case in which you are + * manually feeding module vectors to a swerve module. + *

+ * + *

+ * When using this vector, keep in mind that you are assigning a speed in units + * of percentage output, from -1 to 1, 0 being 0% output, 1 being 100% output in + * the "normal" direction, and -1 being 100% output in the "backwards" + * direction. The angle component should be in degrees, ranging from -360 + * to 360, the direction specifying the direction of rotation. + *

+ * + * @author Jordan Bancino + * @version 5.0.0 + * @since 1.0.0 + */ +@Loggable +public class ModuleVector { + + /** + * Check that a given speed value is within the acceptable bounds, as described + * in this documentation. + * + * @param speed The speed to check. + * @return Whether or not {@code speed} should be accepted as motor controller + * input or not. + */ + public static boolean checkSpeed(double speed) { + return speed <= 1 && speed >= -1; + } + + /** + * Check that a given angle value is within the acceptable bounds, as described + * in this documentation. + * + * @param angle The angle to check. + * @return Whether or not {@code angle} should be accepted as a swerve module + * angle reference or not. + */ + public static boolean checkAngle(double angle) { + return angle <= 360 && angle >= -360; + } + + @Log(atLevel = LogLevel.IMPORTANT) + private double speed, angle; + + /** + * Construct a module vector. This is a convenience constructor for the setter + * methods, and ensures that a module vector is initialized. + * + * @param angle The desired angle of the swerve module, in degrees from 0 to + * 360. + * @param speed The desired speed of the swerve module, in percentage output + * from 0 to 1. + * @throws IllegalArgumentException If the speed or the angle is out of bounds. + */ + public ModuleVector(double angle, double speed) { + setAngle(angle); + setSpeed(speed); + } + + /** + * Duplicate a module vector using {@link #ModuleVector(double, double)}. This + * can be useful when you need to store a vector before passing it down a logic + * chain where it could possibly be modified. + * + * @param v The vector to duplicate. + */ + public ModuleVector(ModuleVector v) { + this(v.getAngle(), v.getSpeed()); + } + + /** + * Set the speed parameter of this module vector. + * + * @param speed The speed parameter, as described above. + * + * @return This instance, for chaining purposes. + */ + public ModuleVector setSpeed(double speed) { + if (checkSpeed(speed)) { + this.speed = speed; + } else { + throw new IllegalArgumentException("Speed out of bounds: " + speed); + } + return this; + } + + /** + * Set the angle parameter of this module vecctor. + * + * @param angle The angle parameter, as described above. + * + * @return This instance, for chaining purposes. + */ + public ModuleVector setAngle(double angle) { + if (checkAngle(angle)) { + this.angle = angle; + } else { + throw new IllegalArgumentException("Angle out of bounds: " + angle); + } + return this; + } + + /** + * Get the speed parameter of this module vector that was set with + * {@link #setSpeed(double)}. + * + * @return The speed value for this vector. + */ + public double getSpeed() { + return speed; + } + + /** + * Get the angle parameter of this module vector that was set with + * {@link #setAngle(double)}. + * + * @return The angle value for this vector. + */ + public double getAngle() { + return angle; + } + + /** + * Add a swerve vector to this one by summing each component. + * + * @param v The vector to add to this one. + * @return The resulting vector from adding all the components together. + */ + public ModuleVector plus(ModuleVector v) { + return new ModuleVector(getAngle() + v.getAngle(), getSpeed() + v.getSpeed()); + } + + /** + * Subtract a swerve vector from this one by finding the difference between each + * component. + * + * @param v The vector to subtract. + * @return The resulting vector from subtracting all the components. + */ + public ModuleVector minus(ModuleVector v) { + return new ModuleVector(getAngle() - v.getAngle(), getSpeed() - v.getSpeed()); + } + + /** + * Multiply a swerve vector by this one by finding the product of each + * component. + * + * @param v The vector to multiply by. + * @return The resulting vector from multiplying all the components. + */ + public ModuleVector times(ModuleVector v) { + return new ModuleVector(getAngle() * v.getAngle(), getSpeed() * v.getSpeed()); + } + + /** + * Divide this swerve vector by another one by dividing each component. + * + * @param v The vector to divide by. + * @return The resulting vector from dividing all the components. + */ + public ModuleVector dividedBy(ModuleVector v) { + return new ModuleVector(getAngle() / v.getAngle(), getSpeed() / v.getSpeed()); + } + + @Override + public String toString() { + return String.format("ModuleVector(angle: %.2f, speed: %.2f)", angle, speed); + } + + @Override + public boolean equals(Object o) { + if (o instanceof ModuleVector) { + ModuleVector v = (ModuleVector) o; + return getSpeed() == v.getSpeed() && getAngle() == getAngle(); + } else { + return false; + } + } +} diff --git a/src/main/java/net/bancino/robotics/swerveio/geometry/SwerveVector.java b/src/main/java/net/bancino/robotics/swerveio/geometry/SwerveVector.java new file mode 100755 index 0000000..810bfe6 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/geometry/SwerveVector.java @@ -0,0 +1,302 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.geometry; + +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; +import net.bancino.robotics.swerveio.SwerveDrive; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** + * A mutable container for a swerve drive vector that holds the forward, strafe, + * and rotation values for a single iteration. This is the core class for + * driving a SwerveIO swerve drive. it is required to drive any swerve drive, + * even autonomously, because WPILib {@code ChassisSpeeds} objects are converted + * to swerve vectors internally. This class represents the chassis + * vector; in other words, it describes the motion of the entire swerve drive as + * one system. + * + *

+ * The swerve vector is intended to be used only by SwerveIO, because SwerveIO + * aims to define a complete API that abstracts everything behind just a few + * WPILib commands and a subsystem. However, there may be cases, such as in + * basic autonomous commands, that you may wish to generate your own swerve + * vectors to pass into the swerve drive. + *

+ * + *

+ * The is a vector, which means it provides velocity, not position. + * SwerveIO has basic positioning support, but nothing beyond what is trivial. + * When using this vector, keep in mind that you are assigning a speed in units + * of percentage output, from -1 to 1, -1 being 100% output in reverse, 0 being + * 0% output, and 1 being 100% output in the positive direction. + *

+ * + *

+ * The directions are defined as follows: + *

+ * + *
    + *
  • FWD: A positive value moves the swerve drive forward
    • + *
  • STR: A positive value moves the swerve drive right
    • + *
  • RCW: A positive value rotates the swerve drive + * clockwise
    • + *
+ * + *

+ * Negative values do the opposite. So, for example, if you wish to move your + * swerve drive to the left and downward, you would provide negative numbers for + * the forward and strafe parameters. + *

+ *

+ * All three axes of this vector are fully independent, which means they can be + * combined in any combination. You should be able to apply all three axes at + * the same time, and the kinematics provider should handle them. See + * {@link net.bancino.robotics.swerveio.kinematics} for how swerve vector values + * are used. Ultimately, while these vectors are passed into the + * {@link net.bancino.robotics.swerveio.SwerveDrive}, they will eventually make + * their way down to a kinematics provider, which is responsible for computing + * individuial module vectors from this overall swerve vector. + *

+ * + * @author Jordan Bancino + * @version 4.2.0 + * @since 1.0.0 + */ +@Loggable +public class SwerveVector { + + /** + * Convert a chassis speed object into a raw swerve vector based on the maximum + * velocity that a swerve drive can achieve. If the passed vector exceeds the + * limits of this swerve drive, driving it with this vector will result in a + * full-throttle. + * + * @param v A WPILib ChassisSpeeds vector that represents the velocity to + * apply to this swerve base. See + * {@link net.bancino.robotics.swerveio.command.PathweaverSwerveDrive}. + * @param swerve A swerve drive. This is required to obtain the maximum speed + * angular velocity, so that the ChassisSpeeds can be scaled to + * the specific swerve. + * @return A swerve vector designed to drive the specified swerve drive at the + * speeds given by the ChassisSpeeds object. + */ + public static SwerveVector fromChassisSpeeds(ChassisSpeeds v, SwerveDrive swerve) { + double maxMPS = swerve.getChassisMaxSpeed(); + SwerveVector swerveVector = new SwerveVector(v.vxMetersPerSecond / maxMPS, v.vyMetersPerSecond / maxMPS, + v.omegaRadiansPerSecond / swerve.getChassisMaxOmega()); + return swerveVector; + } + + /** + * A convenience method that simply calls + * {@link #convertToChassisSpeeds(SwerveDrive)} on the passed vector. This + * method exists solely to remain consistent with + * {@link #fromChassisSpeeds(ChassisSpeeds, SwerveDrive)}. It might be useful + * for querying the absolute velocity in meters per second or radians per second + * that a particular swerve vector will cause the swerve drive to go. + * + * @param v The swerve vector to convert to a ChassisSpeeds object for the + * given swerve drive. + * @param swerve The swerve drive object; used to properly scale the resulting + * speed object. + * @return See {@link #convertToChassisSpeeds(SwerveDrive)} + */ + public static ChassisSpeeds toChassisSpeeds(SwerveVector v, SwerveDrive swerve) { + return v.convertToChassisSpeeds(swerve); + } + + @Log(atLevel = LogLevel.IMPORTANT) + private double fwd, str, rcw; + + /** + * Construct a swerve vector. No checks are done on the passed parameters, so it + * is possible to overrun the boundaries of the swerve drive. However, WPILib's + * motor controllers—where these values will ultimately end up— will + * not overrun the motors, so any value that is out of bounds will effectively + * be treated as 100%. + * + *

+ * This constructor is just a convenience method for calling all the setter + * methods, and prevent uninitialized swerve vectors. + *

+ * + * @param fwd The forward (Y) velocity for this vector. + * @param str The strafe (X) velocity for this vector. + * @param rcw The rotation (Z) velocity for this vector. + */ + public SwerveVector(double fwd, double str, double rcw) { + setFwd(fwd); + setStr(str); + setRcw(rcw); + } + + /** + * Duplicate an existing swerve vector using + * {@link #SwerveVector(double, double, double)}. This can be useful when you + * need to store a vector before passing it down a logic chain where it could + * possibly be modified. + * + * @param v The vector to duplicate. + */ + public SwerveVector(SwerveVector v) { + this(v.getFwd(), v.getStr(), v.getRcw()); + } + + /** + * Set the forward parameter of this swerve vector. + * + * @param fwd The forward parameter, as described above. + * + * @return This instance, for chaining purposes. + */ + public SwerveVector setFwd(double fwd) { + this.fwd = fwd; + return this; + } + + /** + * Set the strafe parameter of this swerve vector. + * + * @param str The strafe parameter, as described above. + * + * @return This instance, for chaining purposes. + */ + public SwerveVector setStr(double str) { + this.str = str; + return this; + } + + /** + * Set the rotational parameter of this swerve vector. + * + * @param rcw The rotational parameter, as described above. + * + * @return This instance, for chaining purposes. + */ + public SwerveVector setRcw(double rcw) { + this.rcw = rcw; + return this; + } + + /** + * Get the forward parameter of this swerve vector that was set with + * {@link #setFwd(double)}. + * + * @return The fwd value for this vector. + */ + public double getFwd() { + return fwd; + } + + /** + * Get the strafe parameter of this swerve vector that set with + * {@link #setStr(double)}. + * + * @return The str value for this vector. + */ + public double getStr() { + return str; + } + + /** + * Get the angular parameter of this swerve vector that set with + * {@link #setRcw(double)}. + * + * @return The rcw value for this vector. + */ + public double getRcw() { + return rcw; + } + + /** + * Convert this vector into a chassis speed vector using a specific swerve + * drive, because the absolute speed of the chassis depends on the maximum + * linear and angular velocity of the swerve drive, since this class merely + * represents percentage of full output. + * + * @param drive The swerve drive to use when converting. This is required + * because the maximum speed and angular speed are used in the + * calculation. + * @return A WPILib chassis speed object that represents the swerve vector + * provided. + */ + public ChassisSpeeds convertToChassisSpeeds(SwerveDrive drive) { + double maxOutputMPS = drive.getChassisMaxSpeed(); + double maxChassisRadiansPerSecond = drive.getChassisMaxOmega(); + return new ChassisSpeeds(getFwd() * maxOutputMPS, getStr() * maxOutputMPS, + getRcw() * maxChassisRadiansPerSecond); + } + + /** + * Add a swerve vector to this one by summing each component. + * + * @param v The vector to add to this one. + * @return The resulting vector from adding all the components together. + */ + public SwerveVector plus(SwerveVector v) { + return new SwerveVector(fwd + v.getFwd(), str + v.getStr(), rcw + v.getRcw()); + } + + /** + * Subtract a swerve vector from this one by finding the difference between each + * component. + * + * @param v The vector to subtract. + * @return The resulting vector from subtracting all the components. + */ + public SwerveVector minus(SwerveVector v) { + return new SwerveVector(fwd - v.getFwd(), str - v.getStr(), rcw - v.getRcw()); + } + + /** + * Multiply a swerve vector by this one by finding the product of each + * component. + * + * @param v The vector to multiply by. + * @return The resulting vector from multiplying all the components. + */ + public SwerveVector times(SwerveVector v) { + return new SwerveVector(fwd * v.getFwd(), str * v.getStr(), rcw * v.getRcw()); + } + + /** + * Divide this swerve vector by another one by dividing each component. + * + * @param v The vector to divide by. + * @return The resulting vector from dividing all the components. + */ + public SwerveVector dividedBy(SwerveVector v) { + return new SwerveVector(fwd / v.getFwd(), str / v.getStr(), rcw / v.getRcw()); + } + + @Override + public String toString() { + return String.format("SwerveVector(fwd: %.3f, str: %.3f, rcw: %.3f)", getFwd(), getStr(), getRcw()); + } + + @Override + public boolean equals(Object o) { + if (o instanceof SwerveVector) { + SwerveVector v = (SwerveVector) o; + return getFwd() == v.getFwd() && getStr() == v.getStr() && getRcw() == v.getRcw(); + } else { + return false; + } + } +} diff --git a/src/main/java/net/bancino/robotics/swerveio/geometry/package-info.java b/src/main/java/net/bancino/robotics/swerveio/geometry/package-info.java new file mode 100755 index 0000000..e1f6b42 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/geometry/package-info.java @@ -0,0 +1,35 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +/** + * Length, Dimensions, and Vectors. The geometry package provides a number of classes + * that represent various geometric properties required in driving a swerve drive. + * The classes in this package are use throughout SwerveIO and are often dependent + * on one another, so it is important to understand them. + * + *

+ * This package does not offer any advanced computations, it merely deals with + * representing geometric and physical structures in a way that is logical and cohesive + * for SwerveIO. For details on how the physics and math of SwerveIO works, see the + * {@link net.bancino.robotics.swerveio.kinematics} package, which details the exact + * implementations used. + *

+ * + * @author Jordan Bancino + * @version 5.0.0 + * @since 2.0.0 + */ +package net.bancino.robotics.swerveio.geometry; \ No newline at end of file diff --git a/src/main/java/net/bancino/robotics/swerveio/gyro/Gyro.java b/src/main/java/net/bancino/robotics/swerveio/gyro/Gyro.java new file mode 100755 index 0000000..d47cebc --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/gyro/Gyro.java @@ -0,0 +1,64 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.gyro; + +/** + * The gyro representation used by SwerveIO. This is a complete definition of + * the gyro API. Every gyro that is used with SwerveIO must implement this + * interface. + * + *

+ * The interface itself is fairly simplistic because there are only a few + * methods and some of them can usually be implemented in a just a few lines of + * code. However, the output of the methods documented below has strict rules. + * Pay careful attention to the documentation for each method and be sure to + * follow the specifications, otherwise things may misbehave in unexpected ways. + *

+ * + * @author Jordan Bancino + * @since 6.0.2 + * @version 2.0.0 + */ +public interface Gyro { + + /** + * Get the gyro's current yaw. Note that this may not always be double + * precision. It could be an integer as well. In most cases, this should be a + * raw value straight from the hardware, however in some obscure cases, + * scaling may need to occur to convert a JNI reading into a degree measure in + * terms of 360. + * + *

+ * This should go from 0 to 360. If the sensor does not return the angle in + * terms of 0 to 360, then this function should convert it. For instance, if the + * sensor returns 0 to -180 and 0 to 180, it should be scaled to 360 here. If + * the gyro angle is continuous—that is, it doesn't reset at all and just + * continues in the positive or negative direction—it should be converted + * to a 0-360 reading. + *

+ * + * @return The yaw angle, in degrees. + */ + public double getAngle(); + + /** + * Zero the gyro. If the gyro hardware does not support this, a software offset + * should be performed in this method so that subsequent calls to + * {@link #getAngle()} are adjusted. This method is not allowed to fail. + */ + public void zero(); +} \ No newline at end of file diff --git a/src/main/java/net/bancino/robotics/swerveio/gyro/WPILibGyro.java b/src/main/java/net/bancino/robotics/swerveio/gyro/WPILibGyro.java new file mode 100755 index 0000000..cf1019d --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/gyro/WPILibGyro.java @@ -0,0 +1,78 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.gyro; + +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; + +/** + * Convert a WPILib gyro to a SwerveIO Gyro. This class wraps a WPILib gyro + * object in an implementation of the SwerveIO gyro class so that it can be used + * with SwerveIO. This can be used as an easy way to add gyro support for any + * FRC gyro that implements the WPILib interface. The reason that SwerveIO does + * not use the WPILib gyro interface is because SwerveIO has strict gyro + * requirements that the WPILib interface does not specify. For example, the + * WPILib interface specifies a method that reports an angle measure without any + * discontinuities. This is fine for some uses, but SwerveIO does not operate on + * angles less than zero or greater than 360, so SwerveIO must scale continuous + * values outside of that range down. That's what this class does. It wraps the + * WPILib continuous angle in a SwerveIO discontinuous angle. It also renames + * the reset() method specified by WPILib Gyro to {@link #zero()}, which more + * accurately describes the purpose of the method. + * + * @author Jordan Bancino + * @version 6.0.2 + * @since 6.0.2 + * + */ +@Loggable +public class WPILibGyro implements Gyro { + + private final edu.wpi.first.wpilibj.interfaces.Gyro gyro; + + @Log(as = "angle", atLevel = LogLevel.IMPORTANT) + private double lastAngle; + + /** + * Construct a new SwerveIO gyro with a WPILib gyro. + * + * @param gyro The WPILib to wrap. + */ + public WPILibGyro(edu.wpi.first.wpilibj.interfaces.Gyro gyro) { + if (gyro != null) { + this.gyro = gyro; + } else { + throw new IllegalArgumentException("Gyro cannot be null."); + } + } + + @Override + public double getAngle() { + double angle = gyro.getAngle() % 360; + if (angle < 0) { + angle += 360; + } + lastAngle = angle; + return angle; + } + + @Override + public void zero() { + gyro.reset(); + } +} diff --git a/src/main/java/net/bancino/robotics/swerveio/gyro/package-info.java b/src/main/java/net/bancino/robotics/swerveio/gyro/package-info.java new file mode 100755 index 0000000..9aad056 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/gyro/package-info.java @@ -0,0 +1,48 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +/** + * Gyro interfaces and helper classes. This package provides an interface for + * adding your own custom gyros. See {@link Gyro} for details on how a gyro is + * expected to behave. + * + *

+ * Beginning users of SwerveIO will probably want to an existing gyro + * implementation, but they can also easily create their own implementations by + * directly implementing {@link Gyro}. + *

+ * This package does not provide any module hardware support itself. Please + * refer to the documentation home page for adding hardware support for your kit + * module. + *

+ *

+ * If your gyro is listed as supported, use the hardware-specific classes to + * access them. The documentation for each individual gyro will provide you with + * all the specifics. If your gyro is not officially supported by SwerveIO, you + * will have to do a little more work to get SwerveIO to play nice with it, but + * due to the nature of SwerveIO and the design principles it was built with, + * adding your own gyro implementation should be trivial. + *

+ *

+ * Using the interfaces and classes in this package, you can add support for any + * gyro. + *

+ * + * @author Jordan Bancino + * @version 6.0.0 + * @since 2.0.0 + */ +package net.bancino.robotics.swerveio.gyro; \ No newline at end of file diff --git a/src/main/java/net/bancino/robotics/swerveio/kinematics/DefaultSwerveKinematics.java b/src/main/java/net/bancino/robotics/swerveio/kinematics/DefaultSwerveKinematics.java new file mode 100755 index 0000000..77495d5 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/kinematics/DefaultSwerveKinematics.java @@ -0,0 +1,137 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.kinematics; + +import net.bancino.robotics.swerveio.module.SwerveModule; +import net.bancino.robotics.swerveio.geometry.SwerveVector; +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; +import net.bancino.robotics.swerveio.geometry.ChassisDimension; +import net.bancino.robotics.swerveio.geometry.ModuleVector; +import net.bancino.robotics.swerveio.geometry.Length.Unit; + +import static java.lang.Math.toDegrees; +import static java.lang.Math.toRadians; +import static java.lang.Math.atan2; +import static java.lang.Math.sin; +import static java.lang.Math.cos; +import static java.lang.Math.sqrt; +import static java.lang.Math.pow; + +import java.util.EnumMap; +import java.util.Map; + +/** + * The default kinematics provider for SwerveIO. These kinematics will drive the + * swerve drive in what's often called "crab mode". This means that all four + * modules are driven independently by four different vectors. + * + *

+ * The exact implementation of this kinematics calculator is beyond the scope of + * this documentation, but the implementation generally follows the pseudocode + * contained in popular, web-searchable swerve drive documents. + * + * @author Jordan Bancino + * @version 4.2.0 + * @since 2.1.0 + */ +@Loggable +public class DefaultSwerveKinematics implements SwerveKinematicsProvider { + + @Log(as = "chassisDimensions") + private ChassisDimension chassis; + + @Log(as = "calculatedVectors", atLevel = LogLevel.IMPORTANT) + private final Map lastVectors = new EnumMap<>(SwerveModule.Location.class); + + /** + * Create a default swerve drive kinematics calculator. This will drive the + * swerve drive using default, sensible behavior. + * + * @param chassis The chassis dimensions to use for these calculations. This is + * required to ensure that the vectors are accurate. This class + * uses these dimensions solely for the ratio of the width to the + * height, but please ensure that the units are correct as well, + * because this dimension object is returned by + * {@link #getChassisDimensions()}, which is used throughout + * SwerveIO. + */ + public DefaultSwerveKinematics(ChassisDimension chassis) { + if (chassis != null) { + this.chassis = chassis; + } else { + throw new IllegalArgumentException("ChassisDimensions cannot be null."); + } + } + + @Override + public ChassisDimension getChassisDimensions() { + return chassis; + } + + @Override + public ModuleVector getModuleVector(SwerveModule.Location module, SwerveVector vector, double gyroAngle) { + double cosAngle = cos(toRadians(gyroAngle)); + double sinAngle = sin(toRadians(gyroAngle)); + + double fwd = vector.getFwd(); + double str = vector.getStr(); + double rcw = vector.getRcw(); + + // Field-centric drive adjustments + double modFwd = (fwd * cosAngle) + (str * sinAngle); + double modStr = (-fwd * sinAngle) + (str * cosAngle); + fwd = modFwd; + str = modStr; + + double vX, vY; + switch (module) { + case FRONT_RIGHT: + vX = str + rcw; + vY = fwd - rcw; + break; + case FRONT_LEFT: + vX = str + rcw; + vY = fwd + rcw; + break; + case REAR_LEFT: + vX = str - rcw; + vY = fwd + rcw; + break; + case REAR_RIGHT: + vX = str - rcw; + vY = fwd - rcw; + break; + default: // Should never happen. + throw new IllegalArgumentException("Unknown swerve module location: " + module); + } + double baseLength = chassis.getLength().get(Unit.BASE_UNIT); + double baseWidth = chassis.getWidth().get(Unit.BASE_UNIT); + double baseDiagonal = chassis.getDiagonal().get(Unit.BASE_UNIT); + + vX *= baseLength / baseDiagonal; + vY *= baseWidth / baseDiagonal; + + double angle = toDegrees(atan2(vX, vY)); + double speed = sqrt(pow(vX, 2) + pow(vY, 2)); + + ModuleVector moduleVector = new ModuleVector(angle, (speed > 1) ? 1 : speed); + lastVectors.put(module, moduleVector); + return moduleVector; + } +} diff --git a/src/main/java/net/bancino/robotics/swerveio/kinematics/SwerveKinematicsProvider.java b/src/main/java/net/bancino/robotics/swerveio/kinematics/SwerveKinematicsProvider.java new file mode 100755 index 0000000..a4491dd --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/kinematics/SwerveKinematicsProvider.java @@ -0,0 +1,109 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.kinematics; + +import net.bancino.robotics.swerveio.module.SwerveModule; +import net.bancino.robotics.swerveio.geometry.SwerveVector; +import net.bancino.robotics.swerveio.geometry.ChassisDimension; +import net.bancino.robotics.swerveio.geometry.ModuleVector; + +/** + * The complete definition of the SwerveIO kinematics API. This interface must + * be implemented by all calculator classes that wish to supply a SwerveIO + * swerve drive with wheel angles and speeds. As described in the package + * documentation, the kinematics API is only responsible for performing the + * theoretical calculations; leave the execution of those calculations to the + * main swerve drive class. + * + *

+ * The method implementations required by this interface should be stateless; + * they should have no memory of past or future data. Each method should merely + * calculate the target angle for the given function call. + *

+ * + * @author Jordan Bancino + * @version 2.1.0 + * @since 2.1.0 + */ +public interface SwerveKinematicsProvider { + + /** + * Provide the chassis dimensions of the swerve drive. These dimensions are + * usually required to effectively drive a swerve drive. Most implementations + * require the dimensions be provided in the constructor; if that is the case, + * the dimensions from the constructor can be returned here. + * + *

+ * If the kinematics provider does not require the chassis dimensions to + * operate, this method still must provide accurate chassis dimensions, + * because the result of this function is used in numerous places throughout + * SwerveIO. + *

+ * + * @return The chassis dimensions that are being used to compute the inverse + * kinematics, or can be used in the future to compute angular velocity. + */ + public ChassisDimension getChassisDimensions(); + + /** + * Compute the module vector for a given swerve module and swerve vector. This + * method is also field-centric aware; it should adjust the vector based on the + * provided gyro angle. + * + *

+ * See the {@link net.bancino.robotics.swerveio.geometry.ModuleVector} + * documentation for the module vector requirements. + *

+ * + * @param module The swerve module to perform the computation for. + * @param swerveVector The swerve drive vector used to calculate the module + * vector. + * @param gyroAngle The gyro angle to modify the vector calculations with. + * This value will always come from + * {@link net.bancino.robotics.swerveio.gyro.Gyro#getAngle()}, + * so be sure to check the documentation for that method for + * the exact standards that this method should adhere to. + * You can reasonably assume that the output of that + * function is correct, and if it isn't, it isn't your + * problem here, it's a gyro implementation problem. It is + * not the job of the kinematics provider to correct invalid + * gyro input. + * @return A valid module vector for driving the provided swerve module. + */ + public ModuleVector getModuleVector(SwerveModule.Location module, SwerveVector swerveVector, double gyroAngle); + + /** + * Compute the module vector for a given swerve module and swerve vector. The + * default implementation of this function calls + * {@link #getModuleVector(Location, SwerveVector, double)} with a gyro + * angle of zero. For most cases, this should behavior should not need to be + * overridden. + * + *

+ * See the {@link net.bancino.robotics.swerveio.geometry.ModuleVector} + * documentation for the module vector requirements. + *

+ * + * @param module The swerve module to perform the computation for. + * @param swerveVector The swerve drive vector used to calculate the module + * vector. + * @return A valid module vector for driving the provided swerve module. + */ + public default ModuleVector getModuleVector(SwerveModule.Location module, SwerveVector swerveVector) { + return getModuleVector(module, swerveVector, 0); + } +} \ No newline at end of file diff --git a/src/main/java/net/bancino/robotics/swerveio/kinematics/package-info.java b/src/main/java/net/bancino/robotics/swerveio/kinematics/package-info.java new file mode 100755 index 0000000..07f8d86 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/kinematics/package-info.java @@ -0,0 +1,45 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +/** + * Kinematics providers implement the theoretical physics behind the Swerve Drive. + * It is the job of {@link net.bancino.robotics.swerveio.SwerveDrive} to interpret + * the vectors produced by the classes in this package, and it is not the job of + * the classes in this package to determin how the vectors it generates should + * be achieved. + * + *

+ * The term "kinematics", as used throughout this package's documentation, actually + * refers to "inverse kinematics". While kinematics takes individual vectors and + * generates a vector for the whole, the classes in this package do the opposite: + * they take a swerve drive vector + * ({@link net.bancino.robotics.swerveio.geometry.SwerveVector}) and convert it + * into individual module speeds and angles. + *

+ * + *

+ * Kinematics providers can have any level of complexity, and can be defined to + * drive a SwerveIO swerve drive in any number of ways. Each implementation should + * be thoroughly documented in it's own class documentation. Implementations can + * be fully "crab mode", or can theoretically drive the swerve drive like a + * tank-drive. + *

+ * + * @author Jordan Bancino + * @version 2.1.0 + * @since 2.1.0 + */ +package net.bancino.robotics.swerveio.kinematics; \ No newline at end of file diff --git a/src/main/java/net/bancino/robotics/swerveio/log/DashboardLog.java b/src/main/java/net/bancino/robotics/swerveio/log/DashboardLog.java new file mode 100755 index 0000000..5f92f8c --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/log/DashboardLog.java @@ -0,0 +1,109 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.log; + +import java.util.List; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import net.bancino.log.LogEntry; +import net.bancino.log.LogHandler; + +/** + * Log to the smart dashboard. This class is used with {@link RobotLogger} to + * log the state of the swerve drive to the dashboard in real time. + * + * @author Jordan Bancino + * @version 6.1.0 + * @since 6.1.0 + */ +public class DashboardLog implements LogHandler { + + private final String rootKey; + + /** + * Construct a new dashboard log with the default root key, "SwerveIO". + */ + public DashboardLog() { + this("SwerveIO"); + } + + /** + * Construct a new dashboard log with a custom root key. + * @param rootKey The root key that all dashboard entries are placed under. + */ + public DashboardLog(String rootKey) { + this.rootKey = rootKey; + } + + @Override + public void accept(LogEntry entry) { + recursiveLog(rootKey, entry); + SmartDashboard.updateValues(); + } + + private void recursiveLog(String parentKey, LogEntry entry) { + if (entry != null) { + parentKey += "/" + entry.getName(); + switch (entry.getType()) { + case LIST: + List list = entry.getAsList(); + Object testVal = list.get(0).getValue(); + if (testVal instanceof Number) { + double[] arr = new double[list.size()]; + for (int i = 0; i < list.size(); i++) { + arr[i] = ((Number) list.get(i).getValue()).doubleValue(); + } + SmartDashboard.putNumberArray(parentKey, arr); + } else if (testVal instanceof Boolean) { + boolean[] arr = new boolean[list.size()]; + for (int i = 0; i < list.size(); i++) { + arr[i] = (boolean) list.get(i).getValue(); + } + SmartDashboard.putBooleanArray(parentKey, arr); + } else { + String[] arr = new String[list.size()]; + for (int i = 0; i < list.size(); i++) { + arr[i] = list.get(i).getValue().toString(); + } + SmartDashboard.putStringArray(parentKey, arr); + } + break; + case NODE: + for (LogEntry child : entry.getChildren()) { + recursiveLog(parentKey, child); + } + break; + case VALUE: + Object val = entry.getValue(); + if (val != null) { + if (val instanceof Boolean) { + SmartDashboard.putBoolean(parentKey, (Boolean) val); + } else if (val instanceof Number) { + SmartDashboard.putNumber(parentKey, ((Number) val).doubleValue()); + } else { + SmartDashboard.putString(parentKey, val.toString()); + } + } else { + SmartDashboard.putString(parentKey, "(null)"); + } + break; + default: + break; + } + } + } +} diff --git a/src/main/java/net/bancino/robotics/swerveio/log/RobotLogger.java b/src/main/java/net/bancino/robotics/swerveio/log/RobotLogger.java new file mode 100755 index 0000000..8c514bc --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/log/RobotLogger.java @@ -0,0 +1,77 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.log; + +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; +import net.bancino.log.Logger; + +/** + * A simple logger designed for iterative logging. As robot code runs + * iteratively, so does the logging method. This class provides a simple wrapper + * around the logger class to ensure that logging occurs at a set interval. + * + * @author Jordan Bancino + * @version 6.1.0 + * @since 6.1.0 + */ +@Loggable +public class RobotLogger extends Logger { + @Log(atLevel = LogLevel.INFO) + private long interval = 0; + + @Log + private long lastTime = 0; + + /** + * Set the interval to log at. Note that this is a fuzzy interval; the actual + * period depends on how often {@link #log(Object)} is called. + * + * @param interval The minimum amount of time, in milliseconds, to place between + * each log. Set this to zero to log as fast as possible. + * @return This logger, for method chaining. + * + * @throws IllegalArgumentException If {@code interval} is less than zero. + */ + public RobotLogger setInterval(long interval) { + if (interval >= 0) { + this.interval = interval; + } else { + throw new IllegalArgumentException("Interval " + interval + " is less than zero ms."); + } + return this; + } + + /** + * Get the logging interval that this logger is using. + * + * @return An interval, in milliseconds, or zero for as fast as possible. + */ + public long getInterval() { + return interval; + } + + @Override + public void log(Object o) throws UnsupportedOperationException { + long time = System.currentTimeMillis(); + if (time - lastTime >= interval) { + super.log(o); + lastTime = time; + } + } +} diff --git a/src/main/java/net/bancino/robotics/swerveio/log/package-info.java b/src/main/java/net/bancino/robotics/swerveio/log/package-info.java new file mode 100755 index 0000000..24465f9 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/log/package-info.java @@ -0,0 +1,24 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +/** + * The SwerveIO logging API that handles outputting log data. + * + * @author Jordan Bancino + * @version 5.0.0 + * @since 1.4.0 + */ +package net.bancino.robotics.swerveio.log; \ No newline at end of file diff --git a/src/main/java/net/bancino/robotics/swerveio/module/GenericSwerveModule.java b/src/main/java/net/bancino/robotics/swerveio/module/GenericSwerveModule.java new file mode 100755 index 0000000..b712178 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/module/GenericSwerveModule.java @@ -0,0 +1,546 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.module; + +import java.util.function.Consumer; +import java.util.function.BiConsumer; + +import edu.wpi.first.wpilibj.motorcontrol.MotorController; +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; +import net.bancino.robotics.swerveio.encoder.Encoder; +import net.bancino.robotics.swerveio.pid.PIDController; +import net.bancino.robotics.swerveio.pid.DefaultPIDController; +import net.bancino.robotics.swerveio.geometry.Length; + +/** + * A generic swerve module class that provides boilerplate module code. This + * class is designed to greatly simplify module implementation development + * because it implements the methods required by {@link SwerveModule} in a way + * that is reusable. + * + *

+ * This class can be instantiated with {@link GenericSwerveModule.Builder}, but + * this should be for prototyping purposes only. While GenericSwerveModule is + * complete and usable on its own, the recommended way to use it is to extend + * it, providing your PID constants and physical property data within the class + * itself, and declaring a constructor that takes CAN IDs, encoder information, + * or other fields that aren't hard-coded. + *

+ *

+ * Swerve module hardware should have dedicated classes that abstract all the + * necessary information from the user. A module kit class provides all the + * constant information for that kit hard-coded into it. Its constructor + * requires only the CAN IDs and encoder IDs to use. + *

+ * + * @author Jordan Bancino + * @version 6.0.0 + * @version 1.0.0 + */ +@Loggable +public class GenericSwerveModule implements SwerveModule { + + /** + * Build a generic swerve module object. This can be used on its own, but as + * described in {@link GenericSwerveModule}, it is primarily intended to be + * instantiated anonymously and passed into the superclass constructor of an + * extending class. + * + *

+ * Because instances of this class are primarily intended to be passed into a + * superclass constructor, it is designed with complete encapsulation in mind. + * Various functions support consumers so that lambda expressions can be used + * for initialization. + *

+ * + */ + public static class Builder { + private MotorController driveMotor = null, pivotMotor = null; + private Encoder pivotEncoder = null, driveEncoder = null; + private double driveGearRatio = 0, driveMaxRPM = 0; + private Length wheelDiameter = null; + private PIDController drivePid, pivotPid; + + private double pivotGearRatio = 1; + private double angleOffset = 0; + + /** + * Set the drive motor for this swerve drive module, optionally providing an + * initializer function that can initialize the motor before it is added. The + * initializer function takes the builder instance that this method was called + * on, as well as the motor that is passed as the first parameter. + * + * @param The WPILib speed controller type. This is automatically + * inferred from the first parameter provided to this function + * and will be the type that is provided to the + * {@code BiConsumer}, so casting is not necessary. + * @param driveMotor The the drive motor add to this builder. + * @param initialize A consumer function that takes this builder, as well as the + * motor that was passed as the first parameter. This is + * primarily intended to be used as the motor initializer. + * There may be times when all initialization needs to occur + * inside the builder, such as when passing an anonymous + * builder into a superclass constructor. + * @return This builder, for command chaining. + */ + public Builder setDriveMotor(S driveMotor, BiConsumer initialize) { + if (initialize != null) { + initialize.accept(this, driveMotor); + } + this.driveMotor = driveMotor; + return this; + } + + /** + * Set the drive motor for this builder. This does not provide an initialization + * interface, it merely assigns the drive motor to the builder. For advanced + * usage, such as performing initialization in the superclass constructor, see + * {@link #setDriveMotor(MotorController, BiConsumer)}. + * + * @param driveMotor The drive motor to assign to this swerve module builder. + * @return This builder, for command chaining. + */ + public Builder setDriveMotor(MotorController driveMotor) { + return setDriveMotor(driveMotor, null); + } + + /** + * Set the pivot motor for this swerve drive module, optionally providing an + * initializer function that can initialize the motor before it is added. The + * initializer function takes the builder instance that this method was called + * on, as well as the motor that is passed as the first parameter. + * + * @param The WPILib speed controller type. This is automatically + * inferred from the first parameter provided to this function + * and will be the type that is provided to the + * {@code BiConsumer}, so casting is not necessary. + * @param pivotMotor The the pivot motor add to this builder. + * @param initialize A consumer function that takes this builder, as well as the + * motor that was passed as the first parameter. This is + * primarily intended to be used as the motor initializer. + * There may be times when all initialization needs to occur + * inside the builder, such as when passing an anonymous + * builder into a superclass constructor. + * @return This builder, for command chaining. + */ + public Builder setPivotMotor(S pivotMotor, BiConsumer initialize) { + if (initialize != null) { + initialize.accept(this, pivotMotor); + } + this.pivotMotor = pivotMotor; + return this; + } + + /** + * Set the pivot motor for this builder. This does not provide an initialization + * interface, it merely assigns the drive motor to the builder. For advanced + * usage, such as performing initialization in the superclass constructor, see + * {@link #setDriveMotor(MotorController, BiConsumer)}. + * + * @param pivotMotor The drive motor to assign to this swerve module builder. + * @return This builder, for command chaining. + */ + public Builder setPivotMotor(MotorController pivotMotor) { + return setPivotMotor(pivotMotor, null); + } + + /** + * Provide the encoder connected to the drive motor. + * + * @param driveEncoder The encoder that monitors the drive motor. + * @return This builder, for command chaining. + */ + public Builder setDriveEncoder(Encoder driveEncoder) { + return setDriveEncoder(driveEncoder, null); + } + + /** + * Provide the encoder connected to the drive motor and initialize it. + * + * @param The encoder type. This is automatically inferred from the + * first parameter and allows you to use the initializer + * function without having to cast the object to your + * encoder type. + * @param driveEncoder The encoder that monitors the drive motor. + * @param initialize The function to run on the encoder. It also provides this + * builder function. + * @return This instance for chaining. + */ + public Builder setDriveEncoder(E driveEncoder, BiConsumer initialize) { + if (initialize != null) { + initialize.accept(this, driveEncoder); + } + this.driveEncoder = driveEncoder; + return this; + } + + /** + * Provide the encoder connected to the pivot motor. + * + * @param pivotEncoder The encoder that monitors the pivot motor. + * @return This builder, for command chaining. + */ + public Builder setPivotEncoder(Encoder pivotEncoder) { + return setPivotEncoder(pivotEncoder, null); + } + + /** + * Provide the encoder connected to the pivot motor and initialize it. + * + * @param The encoder type. This is automatically inferred from the + * first parameter and allows you to use the initializer + * function without having to cast the object to your + * encoder type. + * @param pivotEncoder The encoder that monitors the pivot motor. + * @param initialize The function to run on the encoder. It also provides this + * builder function. + * @return This instance for chaining. + */ + public Builder setPivotEncoder(E pivotEncoder, BiConsumer initialize) { + if (initialize != null) { + initialize.accept(this, pivotEncoder); + } + this.pivotEncoder = pivotEncoder; + return this; + } + + /** + * Set a custom PID controller that is connected to the drive motor. This should + * be used for hardware-level PID controllers, or PID controllers implemented in + * hardware vendor libraries. Calls to + * {@link GenericSwerveModule#getDrivePIDController()} will return this + * controller, so it can be beneficial to properly set this up if you want your + * robot code to be able to modify the PID controller properties. + * + * @param drivePid The PID controller attached to the drive motor. + * @return This builder, for chaining. + */ + public Builder setDrivePIDController(PIDController drivePid) { + this.drivePid = drivePid; + return this; + } + + /** + * Set a custom PID controller that is connected to the pivot motor. This should + * be used for hardware-level PID controllers, or PID controllers implemented in + * hardware vendor libraries. Calls to + * {@link GenericSwerveModule#getPivotPIDController()} will return this + * controller, so it can be beneficial to properly set this up if you want your + * robot code to be able to modify the PID controller properties. + * + * @param pivotPid The PID controller attached to the pivot motor. + * @return This builder, for chaining. + */ + public Builder setPivotPIDController(PIDController pivotPid) { + this.pivotPid = pivotPid; + return this; + } + + /** + * Set the input/output ratio of the drive motor gearbox. This is used for + * determining the maximum velocity of this module. + * + * @param driveGearRatio The decimal value of the gear ratio of the gearbox that + * connects the drive motor to the wheel. + * @return This builder, for command chaining. + */ + public Builder setDriveGearRatio(double driveGearRatio) { + this.driveGearRatio = driveGearRatio; + return this; + } + + /** + * Set the input/output ratio of the pivot motor gearbox. This is used for + * determining how many encoder counts there are per one module revolution, so + * you can ignore this method if you are using a 1:1 encoder. + * + * @param pivotGearRatio The decimal value of the gear ratio of the gearbox that + * connects the pivot motor to the output rotating + * mechanism that controls the swerve module's rotation. + * @return This builder, for command chaining. + */ + public Builder setPivotGearRatio(double pivotGearRatio) { + this.pivotGearRatio = pivotGearRatio; + return this; + } + + /** + * Set the maximum drive motor rotations per minute. This is used for + * determining the maximum velocity of this module. + * + * @param driveMaxRPM The decimal value of the maximum drive rotations per + * minute. This is usually discovered by experimentation + * @return This builder, for command chaining. + */ + public Builder setDriveMaxRPM(double driveMaxRPM) { + this.driveMaxRPM = driveMaxRPM; + return this; + } + + /** + * Set the diameter of the drive wheel. This is used for determining the maximum + * velocity of this module. + * + * @param wheelDiameter The diameter of the wheel, given as a length object so + * you can choose your units. + * @return This builder, for command chaining. + */ + public Builder setWheelDiameter(Length wheelDiameter) { + this.wheelDiameter = wheelDiameter; + return this; + } + + /** + * Get the angle offset that this module should use. This is useful for setting + * the "forward" position of the swerve module. When using absolute encoders, + * you're going to get an angle measure that is aligned with the values those + * encoders output. If your encoder is not zero at the forward position, which + * is nearly impossible, then this should be the angle value at which the swerve + * module is considered to be in the "forward" position. + * + * @param angleOffset The angle measure, in degrees, that represents the + * "forward" position of the swerve module. This is used in + * the calculation performed by + * {@link SwerveModule#getAngle()}, so be sure that it is + * accurate for every swerve module, because + * {@link SwerveModule#getAngle()} is adjusted according to + * this value so that it reports a correct reading. + * @return This builder, for command chaining. + */ + public Builder setAngleOffset(double angleOffset) { + this.angleOffset = angleOffset; + return this; + } + + /** + * Build the generic swerve module using an initialization function. + * + * @param initializer A function that accepts the newly-constructed swerve + * module and initializes it or modifies it before the build + * returns. This is useful for applying a custom setup when + * bulding the module. + * @return An object that represents the swerve module using the provided + * information. + * @throws IllegalArgumentException If there is a configuration error. + */ + public GenericSwerveModule build(Consumer initializer) throws IllegalArgumentException { + GenericSwerveModule module = new GenericSwerveModule(this); + if (initializer != null) { + initializer.accept(module); + } + return module; + } + + /** + * Build the generic swerve module without using an initializer function. + * + * @return An object that represents the swerve module using the provided + * information. + * @throws IllegalArgumentException If there is a configuration error. + */ + public GenericSwerveModule build() throws IllegalArgumentException { + return build(null); + } + } + + private MotorController driveMotor, pivotMotor; + + @Log(atLevel = LogLevel.INFO) + private Encoder pivotEncoder, driveEncoder; + + @Log + private double driveGearRatio, driveMaxRPM; + + @Log + private Length wheelDiameter; + + @Log(atLevel = LogLevel.DEBUG) + private PIDController pivotPid = new DefaultPIDController(), drivePid = new DefaultPIDController(); + + @Log + private double pivotGearRatio; + + @Log + private double angleOffset; + + /** + * Construct a generic swerve module. This can only be used in subclasses. The + * public way to build a generic swerve module is with the builder class. + * + * @param builder The builder to use to construct this swerve module. This will + * most likely be anonymous because the call to this superclass + * constructor must be the first line in the constructor, unless + * you pass a static builder in. + */ + protected GenericSwerveModule(Builder builder) { + if (builder.driveMotor != null) { + this.driveMotor = builder.driveMotor; + } else { + throw new IllegalArgumentException("Drive motor cannot be null."); + } + + if (builder.pivotMotor != null) { + this.pivotMotor = builder.pivotMotor; + } else { + throw new IllegalArgumentException("Pivot motor cannot be null."); + } + + if (builder.driveEncoder != null) { + this.driveEncoder = builder.driveEncoder; + } else { + throw new IllegalArgumentException("Drive encoder cannot be null"); + } + + if (builder.pivotEncoder != null) { + this.pivotEncoder = builder.pivotEncoder; + } else { + throw new IllegalArgumentException("Pivot encoder cannot be null."); + } + + if (builder.driveGearRatio > 0) { + this.driveGearRatio = builder.driveGearRatio; + } else { + throw new IllegalArgumentException("Gear ratio cannot be less than or equal to zero."); + } + + if (builder.driveMaxRPM > 0) { + this.driveMaxRPM = builder.driveMaxRPM; + } else { + throw new IllegalArgumentException("Maximum RPMs cannot be less than or equal to zero."); + } + + if (builder.wheelDiameter != null) { + this.wheelDiameter = builder.wheelDiameter; + } else { + throw new IllegalArgumentException("Wheel diameter cannot be null."); + } + + if (builder.drivePid != null) { + this.drivePid = builder.drivePid; + } + + if (builder.pivotPid != null) { + this.pivotPid = builder.pivotPid; + } else { + /* + * Set sensible defaults for this module, but only if we're using the default + * PID controller. + */ + this.pivotPid.setOutputLimits(1); + this.pivotPid.setP(0.0052); + this.pivotPid.setI(0.000069); + this.pivotPid.setD(0); + } + + if (builder.pivotGearRatio > 0) { + this.pivotGearRatio = builder.pivotGearRatio; + } else { + throw new IllegalArgumentException("Pivot gear ratio must be greater than zero."); + } + + setAngleOffset(builder.angleOffset); + + /* + * Don't let the PID loop output anything greater than the limits of the motor. + */ + pivotPid.setOutputLimits(1); + drivePid.setOutputLimits(1); + + /* Tell the PID loop what the maximum range of the encoder is. */ + pivotPid.setSetpointRange(pivotEncoder.countsPerRevolution()); + } + + @Override + public MotorController getDriveMotor() { + return driveMotor; + } + + @Override + public MotorController getPivotMotor() { + return pivotMotor; + } + + @Override + public Encoder getDriveEncoder() { + return driveEncoder; + } + + @Override + public Encoder getPivotEncoder() { + return pivotEncoder; + } + + @Override + public PIDController getDrivePIDController() { + return drivePid; + } + + @Override + public PIDController getPivotPIDController() { + return pivotPid; + } + + @Override + public double getDriveGearRatio() { + return driveGearRatio; + } + + @Override + public double getPivotGearRatio() { + return pivotGearRatio; + } + + @Override + public double getDriveMaxRPM() { + return driveMaxRPM; + } + + @Override + public Length getWheelDiameter() { + return wheelDiameter; + } + + @Override + public double getAngleOffset() { + return angleOffset; + } + + @Override + public void setAngleOffset(double offset) { + this.angleOffset = offset; + } + + @Override + public boolean equals(Object o) { + if (o instanceof SwerveModule) { + SwerveModule asm = (SwerveModule) o; + return asm.getDriveMaxRPM() == getDriveMaxRPM() && asm.getDriveGearRatio() == getDriveGearRatio() + && asm.getOutputThreshhold() == getOutputThreshhold() + && asm.getWheelDiameter() == getWheelDiameter(); + } else { + return false; + } + } + + @Override + public String toString() { + return "SwerveModule(Drive: " + driveMotor.getClass().getSimpleName() + ", Pivot: " + + pivotMotor.getClass().getSimpleName() + ")"; + } +} \ No newline at end of file diff --git a/src/main/java/net/bancino/robotics/swerveio/module/SwerveModule.java b/src/main/java/net/bancino/robotics/swerveio/module/SwerveModule.java new file mode 100755 index 0000000..706fae9 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/module/SwerveModule.java @@ -0,0 +1,527 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.module; + +import net.bancino.robotics.swerveio.pid.PIDController; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; +import net.bancino.robotics.swerveio.encoder.Encoder; +import net.bancino.robotics.swerveio.geometry.Length; +import net.bancino.robotics.swerveio.geometry.ModuleVector; +import net.bancino.robotics.swerveio.geometry.Length.Unit; + +/** + * The complete definition of a SwerveIO swerve module. Every swerve module + * implementation must implement this interface. A SwerveIO swerve drive + * contains a module map of modules that can be driven using the methods + * described in this interface. + * + *

+ * This interface binds all the components of the a single swerve module + * together. A swerve module consists of the following components, all available + * through this interface: + *

+ *
    + *
  • Pivot Motor
    • + *
  • Pivot Encoder
    • + *
  • Pivot PID controller
    • + *
  • Drive Motor
    • + *
  • Drive Encoder
    • + *
  • Drive PID Controller
    • + *
+ *

+ * This interface is also responsible for providing certain metadata for the + * swerve module. The following information is required for proper operation of + * the swerve drive, because it is used in the kinematic calculations: + *

+ *
    + *
  • Drive gear ratio (input/output)
    • + *
  • Drive output threshhold
    • + *
  • Wheel diameter
    • + *
  • Drive motor max speed in rotations per minute (RPM)
    • + *
+ *

+ * These are all used by the default implementation of {@link #getMaxSpeed()} + * and should be reasonably accurate. For information on how to implement the + * relevant getter methods, see their individual documentation. + *

+ *

+ * This interface is not designed to be implemented directly. See + * {@link GenericSwerveModule} for a basic implementation that provides + * boilerplate code. + *

+ * + * @author Jordan Bancino + * @version 6.0.0 + * @since 1.0.0 + */ +public interface SwerveModule { + + /** + * When manually driving the PID loop, we must modify our feedback to output the + * correct the distance and orientation of PID output. + * + *

+ * Normally, one would not modify the feedback, however this is necessary to + * ensure the PID loop behaves properly, providing the correct outputs in the + * correct direction, since we have to deal with crossing over 180 and of course + * 0. + *

+ * + * @param currentPos The current position, that is, the feedback. + * @param targetPos The setpoint, the desired position. + * @param countsPerPivotRevolution The number of encoder counts in a single + * pivot revolution (360 degrees) + * @return A corrected "feedback" value that should be put into the pivot pid + * loop. + */ + public static double correctPivotFeedback(double currentPos, double targetPos, double countsPerPivotRevolution) { + /* Set up some convenience variables for processing. */ + double quarterPivot = countsPerPivotRevolution / 4.0; + double halfPivot = countsPerPivotRevolution / 2.0; + + /* + * Normally, one would not modify the feedback, however this is necessary to + * ensure the PID loop behaves properly, providing the correct outputs in the + * correct direction, since we have to deal with crossing over 180 and of course + * 0. + */ + double feedbackMod = currentPos; + /* Calculate the error between the setpoint and the feedback. */ + double positionDiff = targetPos - currentPos; + + /* + * Modify the feedback to produce the correct PID output. + */ + if (targetPos < currentPos) { /* If setpoint is less than feedback. */ + if (positionDiff > -halfPivot) { /* If error is greater than -180 degrees. */ + feedbackMod = currentPos; /* Do not modify the feedback. */ + } else { /* Setpoint is greater than feedback. */ + /* + * If setpoint is greater than 90 degrees and the position difference is not + * equal to -180 + */ + if (targetPos > quarterPivot && positionDiff != -halfPivot) { + feedbackMod = -1 * (countsPerPivotRevolution + positionDiff); + } else { + feedbackMod = currentPos - countsPerPivotRevolution; + } + } + } + + /* Setpoint is greater than feedback. */ + if (targetPos > currentPos) { + if (positionDiff > halfPivot) { /* error is greater than 180 */ + feedbackMod = currentPos + countsPerPivotRevolution; + } else { + feedbackMod = currentPos; + } + } + return feedbackMod; + } + + /** + * An enumeration that represents the four swerve modules. This is used in + * {@link net.bancino.robotics.swerveio.SwerveDrive}'s module map to map swerve + * drive module implementations to a human-readable and easily-accessible name. + * + *

+ * Because of the nature of swerve drive, it technically doesn't matter which + * side of the robot is the "front" and which is the "back", but all robots have + * fronts and backs, so just make your swerve drive matches. The physical + * locations are important because the kinematics providers generate per-module + * vectors, so we need some way to differentiate the modules. Referring to + * modules by their location is the easiest way to refer to them. Some swerve + * drive libraries literally refer to modules as 1, 2, 3, and 4, but that is + * unintuitive. + *

+ * + *

+ * This enumeration can be used by methods that operate on one or more swerve + * modules, such as + * {@link net.bancino.robotics.swerveio.SwerveDrive#drive(net.bancino.robotics.swerveio.geometry.SwerveVector)} + * for example. The constant value represents the module that is being operated + * on. + *

+ * + * @see net.bancino.robotics.swerveio.SwerveDrive#getModule(Location) + */ + public static enum Location { + /** + * The swerve module located in the front left corner of the robot. + */ + FRONT_LEFT, + + /** + * The swerve module located in the front right corner of the robot. + */ + FRONT_RIGHT, + + /** + * The swerve module located in the rear left corner of the robot. + */ + REAR_LEFT, + + /** + * The swerve module located in the rear right corner of the robot. + */ + REAR_RIGHT + } + + /** + * Drive the swerve module using the provide module vector. The module vector + * provides bounded and checked speed and angle values. See the documentation + * for it to know how to implement this method, though the default + * implementation should be fine for the overwhelming majority of cases. + * + *

+ * This function can directly pass the speed component of the module vector into + * the drive motor, but it will most likely need to pass the angle into a PID + * controller, and then pass the output of the PID controller into the pivot + * motor. + *

+ * + * @param vector The vector to be used to set the motor speeds in this module. + */ + public default void drive(ModuleVector vector) { + setSpeed(vector.getSpeed()); + setAngle(vector.getAngle()); + } + + /** + * Read the pivot encoder and report the angle that this module is currently set + * to. The default implementation uses the following methods: + * + *
    + *
  • {@link #getPivotGearRatio()}
    • + *
  • {@link #getPivotEncoder()}
    • + *
  • {@link #getAngleOffset()}
    • + *
+ * + * There is no need to override this default implementation because it + * automatically takes care of scaling the raw encoder reading based on the + * number of counts per pivot motor shaft revolution, as well as the pivot + * encoder gear ratio. + * + * @return The angle measure, in degrees, that this swerve module is currently + * facing. This will be within the interval [0,360). + */ + public default double getAngle() { + /* Calculate the actual angle based on the encoder reading. */ + double countsPerPivotRevolution = getPivotEncoder().countsPerRevolution() * getPivotGearRatio(); + double angle = ((360 / countsPerPivotRevolution) * getPivotEncoder().get()) % 360; + + /* + * If the encoder reading is continuous, the angle will go below zero. This will + * fix the angle within the interval [0,360) + */ + if (angle < 0) { + angle += 360; + } + + /* Apply the angle offset. */ + double offset = getAngleOffset(); + if (angle - offset < 0) { + angle += 360; + } + angle -= offset; + + return angle; + } + + /** + * Set the pivot angle of this swerve module. The default implementation uses + * the following methods: + * + *
    + *
  • {@link #getAngle()}
    • + *
  • {@link #getPivotPIDController()}
    • + *
  • {@link #getPivotMotor()}
    • + *
+ * + * There usually is no need to override this default implementation because it + * automatically takes care of adjusting the angle reference as needed before + * passing it into the PID controller, the output of which is passed into the + * pivot motor. + * + * @param angle The angle measure, in degrees, that this swerve module should be + * facing facing. This should be within the interval (-360,360), or + * [0,360). Both behave the same. + * + * @throws IllegalArgumentException If {@code angle} is out of bounds. + */ + public default void setAngle(double angle) { + if (ModuleVector.checkAngle(angle)) { + /* + * If the kinematics provider gives a negative angle, we should convert it to a + * positive angle that is equivalent, because that's what correctPivotFeedback() + * expects. + */ + if (angle < 0) { + angle += 360; + } + + /* Calculate the angle feedback and setpoint */ + double setpoint = angle; + double feedback = getAngle(); + double feedbackMod = correctPivotFeedback(feedback, setpoint, 360); + + /* Pass the feedback and setpoint into the PID controller */ + double outRef = getPivotPIDController().getOutput(feedbackMod, setpoint); + + /* Pass the PID controller output into the pivot motor. */ + getPivotMotor().set(outRef); + } else { + throw new IllegalArgumentException("Angle out of bounds: " + angle); + } + } + + /** + * Get the angle offset that this module should use. This is useful for setting + * the "forward" position of the swerve module. When using absolute encoders, + * you're going to get an angle measure that is aligned with the values those + * encoders output. If your encoder is not zero at the forward position, which + * is nearly impossible, then this should be the angle value at which the swerve + * module is considered to be in the "forward" position. + * + * @return The angle measure, in degrees, that represents the "forward" position + * of the swerve module. This is used in the calculation performed by + * {@link #getAngle()}, so be sure that it is accurate for every swerve + * module, because {@link #getAngle()} is adjusted according to this + * value so that it reports a correct reading. + */ + public double getAngleOffset(); + + /** + * Set the angle offset that this angle should use. This is useful for setting + * the "forward" position of the swerve module. When using absolute encoders, + * you're going to get an angle measure that is aligned with the values those + * encoders output. If your encoder is not zero at the forward position, which + * is nearly impossible, then this should be the angle value at which the swerve + * module is considered to be in the "forward" position. + * + *

+ * This is used for adjusting the offset "on the fly" in the sense that the + * robot program can be running for any duration of time before an offset change + * is requested. Most likely, this will be for re-calibrating the encoders. It + * would not be wise to call this method more than once per robot code lifetime. + *

+ * + * @param offset The angle measure, in degrees, that represents the "forward" + * position of the swerve module. This is used in the calculation + * performed by {@link #getAngle()}, so be sure that it is + * accurate for every swerve module, because {@link #getAngle()} + * is adjusted according to this value so that it reports a + * correct reading. + */ + public void setAngleOffset(double offset); + + /** + * Get the speed of the drive motor for this swerve module. + * + * @return The speed of the drive motor in percent output. This will be within + * the bounds [-1,1]. + */ + public default double getSpeed() { + return getDriveMotor().get(); + } + + /** + * Set the speed of the drive motor for this swerve module. + * + * @param speed The speed of the drive motor in percent output. This should be + * within the bounds [-1, 1]. + * + * @throws IllegalArgumentException If {@code speed} is out of bounds.s + */ + public default void setSpeed(double speed) { + if (ModuleVector.checkSpeed(speed)) { + getDriveMotor().set(speed); + } else { + throw new IllegalArgumentException("Speed out of bounds: " + speed); + } + + } + + /** + * Get the drive motor associated with this swerve module. This allows you to + * collect information on it, but you should never modify it's state, as doing + * so could mess up the operation of the swerve drive. + * + * @return A WPILib SpeedController that represents the drive motor of this + * module, never null. + */ + public MotorController getDriveMotor(); + + /** + * Get the pivot motor associated with this swerve module. This allows you to + * collect information on it, but you should never modify it's state, as doing + * so could mess up the operation of the swerve drive. + * + * @return A WPILib SpeedController that represents the pivot motor of this + * module, never null. + */ + public MotorController getPivotMotor(); + + /** + * Retrieve the encoder attached to the pivot motor. + * + * @return The encoder that is monitoring the pivot motor output, never null, + * because pivot encoders are required. + * @see net.bancino.robotics.swerveio.encoder.Encoder + */ + public Encoder getPivotEncoder(); + + /** + * Retrieve the encoder attached to the drive motor. Since this is not used by + * SwerveIO directly, module implementations may return null if no encoder is + * present, but it is common practice to provide an interface for the integrated + * encoder, if the drive motor is equipped with one. + * + * @return The encoder that is monitoring the drive motor output, or null if + * there isn't one. + * @see net.bancino.robotics.swerveio.encoder.Encoder + */ + public Encoder getDriveEncoder(); + + /** + * Get the PID controller driving the drive motor. Note that this may not always + * be in use if open-loop control is in effect. + * + * @return The PID controller associated with this swerve module's drive motor, + * or null if there isn't one. + */ + public PIDController getDrivePIDController(); + + /** + * Get the PID controller driving the pivot motor. Note that this may not always + * be in use if open-loop control is in effect. + * + * @return The PID controller driving the pivot motor, never null, because a + * position loop is used to set the pivot angle. + */ + public PIDController getPivotPIDController(); + + /** + * The output threshold is the absolute minimum current percentage that a motor + * controller can run on that produces motion. You'll only ever want to override + * this if your module misbehaves at extremely low speeds. The output of this + * method is used to compute the "idle angle" of the pivot motors. A swerve + * module is considered to be "idling" if it's percent output is below this + * threshhold. + * + * @return The absolute minimum current percentage. This value is usually + * discovered by experimentation. The default is 1% current, as this + * seems to be a safe average for FRC motor controllers and motors. + * @see net.bancino.robotics.swerveio.SwerveDrive#setIdleAngle(double, boolean) + */ + public default double getOutputThreshhold() { + return 0.01; + } + + /** + * The gear ratio of the drive motor. This is the ratio of the input + * shaft rotations to the output shaft rotations. For an example, if your drive + * gearbox outputs 1 revolution of the shaft for every 8 motor revolutions, the + * gear ratio is 8. + * + *

+ * This value is used to compute the maximum velocity of this swerve module. + *

+ * + * @return The gear ratio used to drive this module, (input / output) + */ + public double getDriveGearRatio(); + + /** + * The gear ratio of the pivot motor. This is the ratio of the input + * shaft rotations to the output steering rotations. For an example, if your + * pivot gearbox outputs 1 complete swerve module revolution for every 6 motor + * revolutions, the gear ratio is 6. + * + *

+ * This value is used to compute the encoder counts per revolution if the + * encoder is on the input shaft. You can safely use the default value (1) if + * you are using a 1:1 encoder, even if the actual gear ratio isn't 1:1. + *

+ * + * @return The gear ratio of the pivot motor and encoder, (input / output). + */ + public default double getPivotGearRatio() { + return 1; + } + + /** + * Get the maximum realistic output in rotations per minute that this module's + * drive motor can accomplish. Note that this is the rotations of the drive + * motor itself, not the output wheel. In other words, this is rotations + * per minute of the input shaft to the drive gearbox. This can usually be found + * on your motor's spec page, or observed with a tachometer. + * + *

+ * This value is used to compute the maximum velocity of this swerve module. + *

+ * + * @return The maximum rotations per minute of the drive motor's shaft. + */ + public double getDriveMaxRPM(); + + /** + * Get the diameter of the wheel attached to this module. This is used for + * computing the maximum velocity of this swerve module, and should be found + * from a specifications sheet, or measured accurately. + * + * @return The diameter of the wheel attached to this swerve module. + */ + public Length getWheelDiameter(); + + /** + * Get the maximum speed drive speed of this module based on the maximum drive + * RPMs, the wheel diameter, and the gear ratio. The following functions are + * used in the default implementation, and should thus be accurate: + * + *
    + *
  • {@link #getDriveMaxRPM()}
    • + *
  • {@link #getDriveGearRatio()}
    • + *
  • {@link #getWheelDiameter()}
    • + *
+ * + * @return The maximum linear velocity that this module can achieve in meters + * per second, using the provided module specifications. There is no + * reason to override this method, because as long as the values + * provided by the listed functions are accurate, this will accurately + * compute the maximum velocity. + */ + public default double getMaxSpeed() { + /* Rotations per minute -> rotations per second. */ + double maxInputRPS = getDriveMaxRPM() / 60; + /* Convert input revolutions to output revolutions. */ + double maxOutputRPS = maxInputRPS / getDriveGearRatio(); + /* Get the circumference of the wheel. */ + double wheelCircumference = getWheelDiameter().get(Unit.METERS) * Math.PI; + return maxOutputRPS * wheelCircumference; + } + + /** + * Stop the entire module instantly. This just calls {@code stopMotor()} on the + * pivot motor and the drive motor.There is generally no need to override this + * default implementation. + */ + public default void stop() { + getPivotMotor().stopMotor(); + getDriveMotor().stopMotor(); + } +} diff --git a/src/main/java/net/bancino/robotics/swerveio/module/package-info.java b/src/main/java/net/bancino/robotics/swerveio/module/package-info.java new file mode 100755 index 0000000..3e6e063 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/module/package-info.java @@ -0,0 +1,67 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +/** + * Swerve Module interfaces and helper classes. See {@link SwerveModule} for + * details on how a swerve module is expected to behave. + * + *

+ * Beginning users of SwerveIO will want to use an existing swerve module + * implementation, or create their own modules with + * {@link GenericSwerveModule.Builder}, which creates + * {@link GenericSwerveModule} instances that implement {@link SwerveModule}, + * allowing them to be used in the construction of a swerve drive. For details + * on how to construct a swerve drive, see the + * {@link net.bancino.robotics.swerveio} package overview. + *

+ *

+ * This package does not provide any module hardware support itself. Please + * refer to the documentation home page for adding hardware support for your kit + * module. + *

+ *

+ * If your swerve module is listed as supported, use the hardware-specific + * classes to implement your swerve modules. The documentation for each + * individual module class will provide you with all the specifics. If your + * module is not officially supported by SwerveIO, you will have to do a little + * bit more work to get SwerveIO to play nice with it, but due to the nature of + * SwerveIO and the design principles it was build with, adding your own module + * implementation should be trivial. + *

+ *

+ * Using the interfaces and classes in this package, you can add support for + * any swerve module, be it a kit we haven't heard of, or a custom-built + * module. If you go this route, you would be most benefited by extending the + * {@link GenericSwerveModule} class, passing a builder object into the + * superclass constructor using what was provided by your constructor. + * {@link GenericSwerveModule} is a fully-supported implementation of + * {@link SwerveModule}, and the base on which the officially supported modules + * are built, so you can be sure it will work reliably. + *

+ *

+ * {@link GenericSwerveModule} may have some limitations that make it unsuitable + * for your purposes. If, after trying it, you've decided that this is the case, + * you are encouraged to directly implement {@link SwerveModule}. This will + * require most of the boilerplate code that {@link GenericSwerveModule} was + * intended to eliminate, but it certainly provides much greater control over + * your module. + *

+ * + * @author Jordan Bancino + * @version 6.0.0 + * @since 1.0.0 + */ +package net.bancino.robotics.swerveio.module; \ No newline at end of file diff --git a/src/main/java/net/bancino/robotics/swerveio/package-info.java b/src/main/java/net/bancino/robotics/swerveio/package-info.java new file mode 100755 index 0000000..8359d5e --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/package-info.java @@ -0,0 +1,44 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +/** + * The base SwerveIO package that contains the core functionality of SwerveIO. + *

+ * The classes in the package make up the entry point of the SwerveIO API. They + * bind together all the components present in the subpackages and present an + * abstract representation of a swerve drive. There are absolutely zero hardware + * dependencies at this level; everything is abstract. + *

+ *

+ * The SwerveDrive class is the programmer's first contact with SwerveIO. It + * provides a builder for constructing a new swerve drive object capable of + * driving any hardware configuration without modification. + *

+ *

+ * This package also contains all the enumerations used in this library, which + * are used primarily for setting up maps and custom configurations. + *

+ *

+ * If you are new to SwerveIO and are looking for documentation on setting up + * your first swerve drive, head over to the {@code SwerveDrive.Builder} class. + *

+ * + * @author Jordan Bancino + * @version 5.0.0 + * @since 1.0.0 + * @see SwerveDrive.Builder + */ +package net.bancino.robotics.swerveio; diff --git a/src/main/java/net/bancino/robotics/swerveio/pid/DefaultPIDController.java b/src/main/java/net/bancino/robotics/swerveio/pid/DefaultPIDController.java new file mode 100755 index 0000000..e1f02e5 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/pid/DefaultPIDController.java @@ -0,0 +1,455 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.pid; + +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; + +/** + * The default PID controller implementation for SwerveIO. This controller + * implements all the PID controller interface functionality and provides some + * useful features that prevent integral windup and overshoot. It provides sane + * default behavior that should be easy to tune. + * + * @author Jordan Bancino + * @version 6.0.0 + * @since 1.0.0 + */ +@Loggable +public class DefaultPIDController implements PIDController { + + /* + * This is the absolute minimum number of slots a SwerveIO PID controller is + * required to implement. + */ + private static final int SLOT_COUNT = 4; + + @Log + private double[] p = new double[SLOT_COUNT]; + + @Log + private double[] i = new double[SLOT_COUNT]; + + @Log + private double[] d = new double[SLOT_COUNT]; + + @Log + private double[] f = new double[SLOT_COUNT]; + + @Log(atLevel = LogLevel.DEBUG) + private double[] maxIOutput = new double[SLOT_COUNT]; + + @Log(atLevel = LogLevel.DEBUG) + private double[] maxError = new double[SLOT_COUNT]; + + @Log(atLevel = LogLevel.INFO) + private int slot = 0; + + @Log(atLevel = LogLevel.INFO) + private double errorSum[] = new double[SLOT_COUNT]; + + @Log(atLevel = LogLevel.IMPORTANT) + private double acceptableError = -1; + + @Log + private double maxOutput = 0; + + @Log + private double minOutput = 0; + + @Log + private double lastActual = 0; + + @Log + private boolean firstRun = true; + + @Log(atLevel = LogLevel.DEBUG) + private boolean reversed = false; + + @Log(atLevel = LogLevel.INFO) + private double outputRampRate = 0; + + @Log(atLevel = LogLevel.DEBUG) + private double lastOutput = 0; + + @Log + private double outputFilter = 0; + + @Log + private double setpointRange = 0; + + /** + * Create a new PID controller. This initializes all the controller's gains to + * zero. + */ + public DefaultPIDController() { + this(0, 0, 0); + } + + /** + * Create a PID controller. See setP, setI, setD methods for more detailed + * parameters. + * + * @param p Proportional gain. Large if large difference between setpoint and + * target. + * @param i Integral gain. Becomes large if setpoint cannot reach target + * quickly. + * @param d Derivative gain. Responds quickly to large changes in error. Small + * values prevents P and I terms from causing overshoot. + */ + public DefaultPIDController(double p, double i, double d) { + this(p, i, d, 0); + } + + /** + * Create a DefaultPIDConroller class object. See setP, setI, setD, setF methods + * for more detailed parameters. + * + * @param p Proportional gain. Large if large difference between setpoint and + * target. + * @param i Integral gain. Becomes large if setpoint cannot reach target + * quickly. + * @param d Derivative gain. Responds quickly to large changes in error. Small + * values prevents P and I terms from causing overshoot. + * @param f Feed-forward gain. Open loop "best guess" for the output should be. + * Only useful if setpoint represents a rate. + */ + public DefaultPIDController(double p, double i, double d, double f) { + this.p[slot] = p; + this.i[slot] = i; + this.d[slot] = d; + this.f[slot] = f; + checkSigns(slot); + } + + @Override + public void setP(int slot, double p) { + this.p[slot] = p; + checkSigns(slot); + } + + @Override + public void setI(int slot, double i) { + /* + * This scales the accumulated error to avoid output errors and ensure a + * consistent transition. + */ + if (this.i[slot] != 0) { + errorSum[slot] = errorSum[slot] * this.i[slot] / i; + } + if (maxIOutput[slot] != 0) { + maxError[slot] = maxIOutput[slot] / i; + } + this.i[slot] = i; + checkSigns(slot); + } + + @Override + public void setD(int slot, double d) { + this.d[slot] = d; + checkSigns(slot); + } + + @Override + public void setF(int slot, double f) { + this.f[slot] = f; + checkSigns(slot); + } + + @Override + public void setPID(int slot, double p, double i, double d) { + this.p[slot] = p; + this.d[slot] = d; + /* + * the I term has additional calculations, so we need to use it's specific + * method for setting it. + */ + setI(slot, i); + checkSigns(slot); + } + + @Override + public void setPID(int slot, double p, double i, double d, double f) { + setPID(p, i, d); + this.f[slot] = f; + } + + @Override + public void setMaxIOutput(int slot, double maximum) { + /* + * maxError and Izone are similar, but scaled for different purposes. The + * maxError is generated for simplifying math, since calculations against the + * max error are far more common than changing the I term or Izone. + */ + maxIOutput[slot] = maximum; + if (this.i[slot] != 0) { + maxError[slot] = maxIOutput[slot] / this.i[slot]; + } + } + + @Override + public void setOutputLimits(int slot, double minimum, double maximum) { + if (maximum < minimum) + return; + maxOutput = maximum; + minOutput = minimum; + + /* + * Ensure the bounds of the I term are within the bounds of the allowable output + * swing + */ + if (maxIOutput[slot] == 0 || maxIOutput[slot] > (maximum - minimum)) { + setMaxIOutput(slot, maximum - minimum); + } + } + + @Override + public void setAcceptableError(double acceptableError) { + this.acceptableError = acceptableError; + } + + @Override + public double getAcceptableError() { + return this.acceptableError; + } + + @Override + public void setDirection(boolean reversed) { + this.reversed = reversed; + /* Apply to all slots. */ + for (int slot = 0; slot < SLOT_COUNT; slot++) { + checkSigns(slot); + } + } + + @Override + public double getOutput(double actual, double setpoint) { + double output; + double Poutput; + double Ioutput; + double Doutput; + double Foutput; + + /* Ramp the setpoint used for calculations if user has opted to do so */ + if (setpointRange != 0) { + setpoint = constrain(setpoint, actual - setpointRange, actual + setpointRange); + } + + /* Do the simple parts of the calculations */ + double error = setpoint - actual; + + /* + * If the error is within an acceptable amount, act as if there is no error. + */ + if (acceptableError > 0 && Math.abs(error) < acceptableError) { + error = 0; + } + + /* + * Calculate F output. Notice, this depends only on the setpoint, and not the + * error. + */ + Foutput = f[slot] * setpoint; + + /* Calculate P term */ + Poutput = p[slot] * error; + + /* + * If this is our first time running this, we don't actually _have_ a previous + * input or output. For sensor, sanely assume it was exactly where it is now. + * For last output, we can assume it's the current time-independent outputs. + */ + if (firstRun) { + lastActual = actual; + lastOutput = Poutput + Foutput; + firstRun = false; + } + + /* + * Calculate D Term Note, this is negative. This actually "slows" the system if + * it's doing the correct thing, and small values helps prevent output spikes + * and overshoot + */ + Doutput = -d[slot] * (actual - lastActual); + lastActual = actual; + + /* + * The Iterm is more complex. There's several things to factor in to make it + * easier to deal with. + * + * 1. maxIoutput restricts the amount of output contributed by the Iterm. + * + * 2. prevent windup by not increasing errorSum if we're already running against + * our max Ioutput + * + * 3. prevent windup by not increasing errorSum if output is output=maxOutput + */ + Ioutput = i[slot] * errorSum[slot]; + if (maxIOutput[slot] != 0) { + Ioutput = constrain(Ioutput, -maxIOutput[slot], maxIOutput[slot]); + } + + /* And, finally, we can just add the terms up */ + output = Foutput + Poutput + Ioutput + Doutput; + + /* Figure out what we're doing with the error. */ + if (minOutput != maxOutput && !bounded(output, minOutput, maxOutput)) { + errorSum[slot] = error; + /* + * reset the error sum to a sane level Setting to current error ensures a smooth + * transition when the P term decreases enough for the I term to start acting + * upon the controller From that point the I term will build up as would be + * expected + */ + } else if (outputRampRate != 0 && !bounded(output, lastOutput - outputRampRate, lastOutput + outputRampRate)) { + errorSum[slot] = error; + } else if (maxIOutput[slot] != 0) { + errorSum[slot] = constrain(errorSum[slot] + error, -maxError[slot], maxError[slot]); + /* + * In addition to output limiting directly, we also want to prevent I term + * buildup, so restrict the error directly + */ + } else { + errorSum[slot] += error; + } + + /* Restrict output to our specified output and ramp limits */ + if (outputRampRate != 0) { + output = constrain(output, lastOutput - outputRampRate, lastOutput + outputRampRate); + } + if (minOutput != maxOutput) { + output = constrain(output, minOutput, maxOutput); + } + if (outputFilter != 0) { + output = lastOutput * outputFilter + output * (1 - outputFilter); + } + + lastOutput = output; + return output; + } + + @Override + public void reset() { + firstRun = true; + for (int i = 0; i < SLOT_COUNT; i++) { + errorSum[i] = 0; + } + } + + @Override + public void setOutputRampRate(double rate) { + outputRampRate = rate; + } + + @Override + public void setSetpointRange(double range) { + setpointRange = range; + } + + @Override + public void setOutputFilter(double strength) { + if (strength == 0 || bounded(strength, 0, 1)) { + outputFilter = strength; + } + } + + /** + * Forces a value into a specific range + * + * @param value input value + * @param min maximum returned value + * @param max minimum value in range + * @return Value if it's within provided range, min or max otherwise + */ + private double constrain(double value, double min, double max) { + if (value > max) { + return max; + } + if (value < min) { + return min; + } + return value; + } + + /** + * Test if the value is within the min and max, inclusive + * + * @param value to test + * @param min Minimum value of range + * @param max Maximum value of range + * @return true if value is within range, false otherwise + */ + private boolean bounded(double value, double min, double max) { + /* + * Note, this is an inclusive range. This is so tests like + * `bounded(constrain(0,0,1),0,1)` will return false. This is more helpful for + * determining edge-case behaviour than <= is. + */ + return (min < value) && (value < max); + } + + /** + * To operate correctly, all PID parameters require the same sign This should + * align with the {@literal}reversed value + */ + private void checkSigns(int slot) { + if (reversed) { /* all values should be below zero */ + if (p[slot] > 0) + p[slot] *= -1; + if (i[slot] > 0) + i[slot] *= -1; + if (d[slot] > 0) + d[slot] *= -1; + if (f[slot] > 0) + f[slot] *= -1; + } else { /* all values should be above zero */ + if (p[slot] < 0) + p[slot] *= -1; + if (i[slot] < 0) + i[slot] *= -1; + if (d[slot] < 0) + d[slot] *= -1; + if (f[slot] < 0) + f[slot] *= -1; + } + } + + @Override + public double getPreviousOutput() { + return lastOutput; + } + + @Override + public void setActiveProfile(int slot) { + if (slot >= 0 && slot < SLOT_COUNT) { + this.slot = slot; + } else { + throw new IllegalArgumentException("Profile slot out of bounds: " + slot); + } + } + + @Override + public int getActiveProfile() { + return slot; + } + + @Override + public int getProfileCount() { + return SLOT_COUNT; + } +} diff --git a/src/main/java/net/bancino/robotics/swerveio/pid/PIDController.java b/src/main/java/net/bancino/robotics/swerveio/pid/PIDController.java new file mode 100755 index 0000000..30c1a90 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/pid/PIDController.java @@ -0,0 +1,393 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.pid; + +/** + * The PID Controller API used by SwerveIO. This is a complete definition, and + * every PID controller used with SwerveIO must implement this interface. + * + *

+ * All PID controllers should generally behave the same way, but this API makes + * zero gaurantees. There is little reason to stray beyond the trusted and + * well-tested {@link DefaultPIDController} controller, but SwerveIO is designed + * to be as extensible as possible, providing interfaces for every pluggable + * aspect of it. + *

+ * + *

+ * This PID controller interface supports multiple profile slots, which allows + * you to set multiple gains on a single controller and switch between them at + * will. PID controllers should provide at least 4 profile slots, but the actual + * number of supported profiles may vary across implementations. + *

+ * + *

+ * It is not recommended to simply wrap hardware vendor-specific PID + * controllers such as on the Spark Max or Talon for pivot motors, because these + * controllers often don't provide the required functionality. Any + * vendor-specific PID controllers are intended to be used for velocity loops on + * the drive motors only. + *

+ * + * @author Jordan Bancino + * @version 6.0.2 + * @since 1.2.3 + */ +public interface PIDController { + + /** + * Set the proportional gain of the PID loop coefficient. (The motor will + * correct itself proportional to the offset of the measure compared to its + * targeted measure.) It's only weakness will be when it treats positive and + * negative offset equally, otherwise it narrows error to 0. + * + * @param p Proportional gain value. Must be positive. This gain is set on the + * active profile slot. + */ + public default void setP(double p) { + setP(getActiveProfile(), p); + } + + /** + * Set the proportional gain of the PID loop coefficient. (The motor will + * correct itself proportional to the offset of the measure compared to its + * targeted measure.) It's only weakness will be when it treats positive and + * negative offset equally, otherwise it narrows error to 0. + * + * @param slot The profile slot to set the gain for. + * @param p Proportional gain value. Must be positive. + */ + public void setP(int slot, double p); + + /** + * Set the integral gain of the PID loop coefficient. (The motor will correct + * itself based on past errors and integrates the history of offset to narrow + * error down to 0.) The downside of using a purely integral system is that it's + * slow to start, as it takes time to accumulate enough information to + * accurately form it's coefficient. + * + * @param i Integral gain value. Must be positive. This gain is set on the + * active profile slot. + */ + public default void setI(double i) { + setI(getActiveProfile(), i); + } + + /** + * Set the integral gain of the PID loop coefficient. (The motor will correct + * itself based on past errors and integrates the history of offset to narrow + * error down to 0.) The downside of using a purely integral system is that it's + * slow to start, as it takes time to accumulate enough information to + * accurately form it's coefficient. + * + * @param slot The profile slot to set the gain for. + * @param i Integral gain value. Must be positive. + */ + public void setI(int slot, double i); + + /** + * Set the derivative gain of the PID loop coefficient. (The motor will correct + * error based on it's rate of change, not seeking to bring the error to 0, but + * seeking to keep the error that the system is stable.) A derivative loop will + * never bring your error to 0, but will simply keep your error from growing + * larger by catching any change and correcting it. + * + * @param d Derivative gain value. Must be positive. This gain is set on the + * active profile slot. + */ + public default void setD(double d) { + setD(getActiveProfile(), d); + } + + /** + * Set the derivative gain of the PID loop coefficient. (The motor will correct + * error based on it's rate of change, not seeking to bring the error to 0, but + * seeking to keep the error that the system is stable.) A derivative loop will + * never bring your error to 0, but will simply keep your error from growing + * larger by catching any change and correcting it. + * + * @param slot The profile slot to set the gain for. + * @param d Derivative gain value. Must be positive. + */ + public void setD(int slot, double d); + + /** + * Sets the priority held in feed-forward augment. In a closed loop system, the + * feed-forward predicts the outcome of the next output from the motor + * controllers for better error correcting accuracy. + * + * @param f Feed-forward gain value. Must be positive. This gain is set on the + * active profile slot. + */ + public default void setF(double f) { + setF(getActiveProfile(), f); + } + + /** + * Sets the priority held in feed-forward augment. In a closed loop system, the + * feed-forward predicts the outcome of the next output from the motor + * controllers for better error correcting accuracy. + * + * @param slot The profile slot to set the gain for. + * @param f Feed-forward gain value. Must be positive. + */ + public void setF(int slot, double f); + + /** + * A convenience function for setting PID values all at once. See {@link #setP}, + * {@link #setI}, and {@link #setD}. This sets the gains on the active profile + * slot. + * + * @param p The value to feed into {@link #setP}. + * @param i The value to feed into {@link #setI}. + * @param d The value to feed into {@link #setD}. + */ + public default void setPID(double p, double i, double d) { + setPID(getActiveProfile(), p, i, d); + } + + /** + * A convenience function for setting PID values all at once. See + * {@link #setP(int,double)}, {@link #setI(int,double)}, and + * {@link #setD(int,double)}. + * + * @param slot The profile slot to set the gains on. + * @param p The value to feed into {@link #setP(int,double)}. + * @param i The value to feed into {@link #setI(int,double)}. + * @param d The value to feed into {@link #setD(int,double)}. + */ + public default void setPID(int slot, double p, double i, double d) { + setP(slot, p); + setI(slot, i); + setD(slot, d); + } + + /** + * A convenience function for setting PID values all at once. See + * {@link #setP(int,double)}, {@link #setI(int,double)}, + * {@link #setD(int,double)}, and {@link #setF(int,double)}. This sets the gains + * on the active profile slot. + * + * @param p The value to feed into {@link #setP(int,double)}. + * @param i The value to feed into {@link #setI(int,double)}. + * @param d The value to feed into {@link #setD(int,double)}. + * @param f The value to feed into {@link #setF(int,double)}. + */ + public default void setPID(double p, double i, double d, double f) { + setPID(getActiveProfile(), p, i, d, f); + } + + /** + * A convenience function for setting PID values all at once. See + * {@link #setP(int,double)}, {@link #setI(int,double)}, + * {@link #setD(int,double)}, and {@link #setF(int,double)}. + * + * @param slot The profile slot to set the gains on. + * @param p The value to feed into {@link #setP(int,double)}. + * @param i The value to feed into {@link #setI(int,double)}. + * @param d The value to feed into {@link #setD(int,double)}. + * @param f The value to feed into {@link #setF(int,double)}. + */ + public default void setPID(int slot, double p, double i, double d, double f) { + setPID(slot, p, i, d); + setF(slot, f); + } + + /** + * Set an acceptable error. The PID controller should output nothing if the + * setpoint and feedback error is within +- the provided error. + * + *

+ * A simple implementation is to take the absolute value of the difference + * between the setpoint and the feedback and check that that value is less than + * this acceptable error. + *

+ * + * @param acceptableError The amount of error allowed by this PID controller. + * This value is in the units of the feedback device and + * applies to both sides of the setpoint. For example, + * when running a controller on a gyro, an acceptable + * error value of 0.5 would allow for 0.5 degrees on both + * sides of the setpoint, which means there is a total + * allowable error of 1 degree. + */ + public void setAcceptableError(double acceptableError); + + /** + * Get the acceptable error that this PID controller allows. + * + * @return See {@link #setAcceptableError(double)}. + */ + public double getAcceptableError(); + + /** + * Set the maximum output value contributed by the I component of the system + * This can be used to prevent large windup issues and make tuning simpler + * + * @param slot The profile slot to get the gains for. + * @param max Units are the same as the expected output value + */ + public void setMaxIOutput(int slot, double max); + + /** + * Set the maximum output value contributed by the I component of the system + * This can be used to prevent large windup issues and make tuning simpler + * + * @param max Units are the same as the expected output value. This is set on + * the active profile slot. + */ + public default void setMaxIOutput(double max) { + setMaxIOutput(getActiveProfile(), max); + } + + /** + * Set the active profile slot on this PID controller. Future calls to methods + * that don't accept a slot parameter use this slot number. + * + * @param slot A profile slot number, greater than or equal to zero. + * + * @throws IllegalArgumentException if {@code slot} is out of bounds for the + * controller. Consult your controller's + * documentation, but the bounds should be + * at minimum 4 slots, giving you slots + * 0, 1, 2, and 3. + */ + public void setActiveProfile(int slot); + + /** + * Get the active profile slot on this PID controller. + * + * @return The active profile slot that this PID controller is pulling + * parameters from. + */ + public int getActiveProfile(); + + /** + * Get the number of profile slots that this PID controller has. It should be at + * least 4, but can be more depending on the implementation. + * + * @return The number of profiles this PID controller supports. + */ + public int getProfileCount(); + + /** + * Set the output limits of the pivot PID controller. + * + * @param slot The profile slot to set the limit values on. + * @param min The minimum value the PID should output. + * @param max The maximum value the PID should output. + */ + public void setOutputLimits(int slot, double min, double max); + + /** + * Set the output limits of the pivot PID controller. + * + * @param slot The profile slot to set the limit on. + * @param output Will produce an output limit of -output to +output. + */ + public default void setOutputLimits(int slot, double output) { + setOutputLimits(slot, -output, output); + } + + /** + * Set the output limits of the pivot PID controller. This limit is applied to + * the active profile slot. + * + * @param output Will produce an output limit of -output to +output. + */ + public default void setOutputLimits(double output) { + setOutputLimits(getActiveProfile(), output); + } + + /** + * A simple toggle for reversing the PID loop's output. This is applied to all + * profile slots. + * + * @param reversed Whether or not to flip the PID output. + */ + public void setDirection(boolean reversed); + + /** + * This is the main function of the PID loop. It performs all the calculations + * to generate PID output. It uses the actively set profile slot's gains in the + * calculations, so make sure the profile you want is set with + * {@link #setActiveProfile(int)} + * + * @param feedback The output feedback; your current reference; where you are + * currently. + * @param setpoint The destination location. This is where you want to be. + * @return The output of the PID calculation given the input. + */ + public double getOutput(double feedback, double setpoint); + + /** + * Get the previous output from the PID loop. This should store whatever the + * previous call to the output function generates. + * + * @return The previous output of the PID calculation. + */ + public double getPreviousOutput(); + + /** + * Reset the controller, erasing the previous output, feedback, and setpoints, + * if stored. This may not do anything at all on certain implementations, but is + * available for implementations to require when the system exits closed-loop + * control for a period of time and then re-enters it. Optionally, if the active + * profile slot changes, implementations may want to reset their internal state + * as well. + */ + public void reset(); + + /** + * Sets the ramp rate for the output. This is applied to all profile slots. + * + * @param rate The ramp rate for the output, in the same units as the output + * value. For each scan, the output value will jump no more than + + * or - this rate. + */ + public void setOutputRampRate(double rate); + + /** + * Set the output range of this PID controller. This effectively scales the + * controller so it knows the bounds of what it will be fed. This is applied to + * all profile slots. + * + * @param range The setpoint input range. In relation to swerve drive, this will + * be the counts per revolution of your pivot encoder, and will + * have no limit on the drive motor. + */ + public void setSetpointRange(double range); + + /** + * Set a filter on the output to reduce sharp oscillations.
+ * 0.1 is likely a sane starting value. Larger values use historical data more + * heavily, with low values weigh newer data. 0 will disable, filtering, and use + * only the most recent value.
+ * Increasing the filter strength will P and D oscillations, but force larger I + * values and increase I term overshoot.
+ * Uses an exponential wieghted rolling sum filter, according to a simple
+ * output*(1-strength)*sum(0..n){output*strength^n} algorithm. + * + *

+ * This value is applied to all profile slots. + *

+ * + * @param strength valid between [0..1), meaning [current output only.. + * historical output only) + */ + public void setOutputFilter(double strength); +} diff --git a/src/main/java/net/bancino/robotics/swerveio/pid/package-info.java b/src/main/java/net/bancino/robotics/swerveio/pid/package-info.java new file mode 100755 index 0000000..edbe6e2 --- /dev/null +++ b/src/main/java/net/bancino/robotics/swerveio/pid/package-info.java @@ -0,0 +1,38 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +/** + * PID controllers used in SwerveIO. This package provides an interface for + * adding your own custom PID controllers. See {@link PIDController} for details + * on how a PID controller is expected to behave. + * + *

+ * Most users of SwerveIO will probably want to use the official PID controller + * of SwerveIO ({@link DefaultPIDController}), but you can also easily create + * your own implementation by directly implementing {@link PIDController}. + *

+ * + *

+ * The PID controller is generally used internally by swerve module + * implementors. The average user may never need to come into contact with a PID + * controller, except to tune it using the PID API specified by this package. + *

+ * + * @author Jordan Bancino + * @version 5.0.0 + * @since 1.2.0 + */ +package net.bancino.robotics.swerveio.pid; \ No newline at end of file diff --git a/src/test/java/net/bancino/robotics/swerveio/geometry/ChassisDimensionTest.java b/src/test/java/net/bancino/robotics/swerveio/geometry/ChassisDimensionTest.java new file mode 100755 index 0000000..702eec6 --- /dev/null +++ b/src/test/java/net/bancino/robotics/swerveio/geometry/ChassisDimensionTest.java @@ -0,0 +1,66 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.geometry; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; + +import org.junit.jupiter.api.Test; + +import net.bancino.robotics.swerveio.geometry.Length.Unit; + +/** + * Test the chassis dimension class. + * + * @author Jordan Bancino + * @version 6.1.0 + * @since 6.1.0 + */ +public class ChassisDimensionTest { + + private ChassisDimension testDimension1 = new ChassisDimension(new Length(25, Unit.INCHES)); + private ChassisDimension testDimension2 = new ChassisDimension(new Length(25, Unit.INCHES), new Length(25, Unit.INCHES)); + private ChassisDimension testDimension3 = new ChassisDimension(new Length(0.5, Unit.METERS)); + + /** + * Test {@code equals()}. + */ + @Test + public void testEquals() { + assertEquals(testDimension1, testDimension2); + assertEquals(testDimension3, new ChassisDimension(new Length(50, Unit.CENTIMETERS))); + } + + /** + * Test that the constructor thows errors on invalid input. + */ + @Test + public void testConstructor() { + assertThrows(IllegalArgumentException.class, () -> new ChassisDimension(null)); + } + + /** + * The the geometry functions. + */ + @Test + public void testGeometry() { + assertEquals(testDimension1.getRadius().get(Unit.BASE_UNIT), testDimension1.getDiagonal().get(Unit.BASE_UNIT) / 2); + assertEquals(Math.hypot(25, 25), testDimension1.getDiagonal().get(Unit.INCHES)); + assertEquals(Math.hypot(0.5, 0.5) / 2.0, testDimension3.getRadius().get(Unit.METERS)); + } + +} diff --git a/src/test/java/net/bancino/robotics/swerveio/geometry/LengthTest.java b/src/test/java/net/bancino/robotics/swerveio/geometry/LengthTest.java new file mode 100755 index 0000000..a7ab20c --- /dev/null +++ b/src/test/java/net/bancino/robotics/swerveio/geometry/LengthTest.java @@ -0,0 +1,90 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.geometry; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; + +import org.junit.jupiter.api.Test; + +import net.bancino.robotics.swerveio.geometry.Length.Unit; + +/** + * Test the length class. + * + * @author Jordan Bancino + * @version 6.1.0 + * @since 6.1.0 + */ +public class LengthTest { + + private Length testLength1 = new Length(0, Unit.CENTIMETERS); + private Length testLength2 = new Length(4, Unit.INCHES); + private Length testLength3 = new Length((double) 1 / 3, Unit.FEET); + private Length testLength4 = new Length(2, Unit.METERS); + private Length testLength5 = new Length(8, Unit.INCHES); + + /** + * Test {@code equals()}. + */ + @Test + public void testEquals() { + assertEquals(testLength2, testLength2); + assertEquals(testLength2, new Length(4, Unit.INCHES)); + assertEquals(testLength2, testLength3); + + assertNotEquals(testLength1, testLength2); + assertNotEquals(testLength3, testLength4); + } + + /** + * Test that the constructor throws errors in invalid input and accepts valid + * input. + */ + @Test + public void testBounds() { + assertThrows(IllegalArgumentException.class, () -> new Length(-1, Unit.FEET)); + assertThrows(IllegalArgumentException.class, () -> new Length(1 - 1.000001, Unit.CENTIMETERS)); + + assertDoesNotThrow(() -> new Length(1, Unit.FEET)); + assertDoesNotThrow(() -> new Length(0.00001, Unit.CENTIMETERS)); + } + + /** + * Test various conversions. + */ + @Test + public void testConversions() { + assertEquals(testLength2.get(Unit.INCHES), 4); + assertEquals(testLength2.get(Unit.FEET), (double) 1 / 3); + assertEquals(testLength3.get(Unit.INCHES), 4); + assertEquals(testLength4.get(Unit.CENTIMETERS), 200); + } + + /** + * Test the mathematical functions. + */ + @Test + public void testMath() { + assertEquals(testLength1, testLength2.minus(testLength3)); + assertEquals(testLength5, testLength3.plus(testLength2)); + assertEquals(testLength5, testLength2.times(new Length(2, Unit.INCHES))); + assertEquals(testLength5, testLength2.dividedBy(new Length(0.5, Unit.INCHES))); + } +} diff --git a/src/test/java/net/bancino/robotics/swerveio/geometry/ModuleVectorTest.java b/src/test/java/net/bancino/robotics/swerveio/geometry/ModuleVectorTest.java new file mode 100755 index 0000000..9388068 --- /dev/null +++ b/src/test/java/net/bancino/robotics/swerveio/geometry/ModuleVectorTest.java @@ -0,0 +1,77 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.geometry; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; + +import org.junit.jupiter.api.Test; + +/** + * Test the module vector class. + * + * @author Jordan Bancino + * @version 6.1.0 + * @since 6.1.0 + */ +public class ModuleVectorTest { + + private ModuleVector testVector1 = new ModuleVector(0, 0); + private ModuleVector testVector2 = new ModuleVector(90, 0.25); + private ModuleVector testVector3 = new ModuleVector(180, 0.5); + + /** + * Test that the bounding checks are working properly. The module vector should + * not allow values outside of the range [-360,+360] for angles and [-1,+1] for + * speeds. + */ + @Test + public void testBounds() { + assertDoesNotThrow(() -> new ModuleVector(0, 0)); + assertDoesNotThrow(() -> new ModuleVector(90, 0.5)); + assertDoesNotThrow(() -> new ModuleVector(358, 1)); + assertDoesNotThrow(() -> new ModuleVector(-45, -0.4)); + + assertThrows(IllegalArgumentException.class, () -> new ModuleVector(-361, 0)); + assertThrows(IllegalArgumentException.class, () -> new ModuleVector(180, -1.001)); + assertThrows(IllegalArgumentException.class, () -> new ModuleVector(365, 1.1)); + } + + /** + * Test that basic math operators work as intended. + */ + @Test + public void testMath() { + assertEquals(testVector3, testVector2.plus(testVector2)); + assertEquals(testVector2, testVector3.minus(testVector2)); + + } + + /** + * Test {@code equals()}. + */ + @Test + public void testEquals() { + assertEquals(testVector1, testVector1); + assertEquals(testVector1, new ModuleVector(0, 0)); + + assertNotEquals(testVector1, testVector2); + assertNotEquals(testVector2, testVector1); + } +} diff --git a/src/test/java/net/bancino/robotics/swerveio/geometry/SwerveVectorTest.java b/src/test/java/net/bancino/robotics/swerveio/geometry/SwerveVectorTest.java new file mode 100755 index 0000000..9bbedce --- /dev/null +++ b/src/test/java/net/bancino/robotics/swerveio/geometry/SwerveVectorTest.java @@ -0,0 +1,62 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.geometry; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; + +import org.junit.jupiter.api.Test; + +/** + * Test the swerve vector class. + * + * @author Jordan Bancino + * @version 6.1.0 + * @since 6.1.0 + */ +public class SwerveVectorTest { + + private SwerveVector testVector1 = new SwerveVector(0, 0, 0); + private SwerveVector testVector2 = new SwerveVector(0.25, 0.25, 0.25); + private SwerveVector testVector3 = new SwerveVector(0.5, 0.5, 0.5); + private SwerveVector testVector4 = new SwerveVector(2, 2, 2); + + /** + * Test the math operators ({@code plus()}, {@code minus()}, {@code times()}, + * {@code dividedBy()}). + */ + @Test + public void testMath() { + assertEquals(testVector3, testVector2.plus(testVector2)); + assertEquals(testVector2, testVector3.minus(testVector2)); + assertEquals(testVector3, testVector2.times(testVector4)); + assertEquals(testVector2, testVector3.dividedBy(testVector4)); + } + + /** + * Test {@code equals()}. + */ + @Test + public void testEquals() { + assertEquals(testVector1, testVector1); + assertEquals(testVector1, new SwerveVector(0, 0, 0)); + + assertNotEquals(testVector1, testVector2); + assertNotEquals(testVector2, testVector1); + assertNotEquals(testVector3, testVector4); + } +} diff --git a/vendor/ctre/build.gradle b/vendor/ctre/build.gradle new file mode 100755 index 0000000..8226207 --- /dev/null +++ b/vendor/ctre/build.gradle @@ -0,0 +1,29 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +/* The CTRE Phoenix API. */ +dependencies { + def phoenixVersion = '5.20.2' + api "com.ctre.phoenix:api-java:${phoenixVersion}" + api "com.ctre.phoenix:wpiapi-java:${phoenixVersion}" + api "com.ctre.phoenix:cci:${phoenixVersion}" + + api "com.ctre.phoenix.sim:cci-sim:${phoenixVersion}" + api "com.ctre.phoenix.sim:simTalonSRX:${phoenixVersion}" + api "com.ctre.phoenix.sim:simVictorSPX:${phoenixVersion}" +} + +/* CTRE uses Doxygen instead of JavaDoc, so we can't link javadoc here. */ diff --git a/vendor/ctre/src/javadoc/overview.html b/vendor/ctre/src/javadoc/overview.html new file mode 100755 index 0000000..ea39be8 --- /dev/null +++ b/vendor/ctre/src/javadoc/overview.html @@ -0,0 +1,12 @@ + + + + +

This API adds official SwerveIO support for CTR Electronics' + hardware. + You should have been directed here from the official SwerveIO documentation. If you weren't, + please go there now for instructions on getting this API set up for use. +

+ + + \ No newline at end of file diff --git a/vendor/ctre/src/main/java/net/bancino/robotics/swerveio/encoder/PhoenixCANCoder.java b/vendor/ctre/src/main/java/net/bancino/robotics/swerveio/encoder/PhoenixCANCoder.java new file mode 100755 index 0000000..0043ef2 --- /dev/null +++ b/vendor/ctre/src/main/java/net/bancino/robotics/swerveio/encoder/PhoenixCANCoder.java @@ -0,0 +1,74 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.encoder; + +import com.ctre.phoenix.sensors.AbsoluteSensorRange; +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.SensorInitializationStrategy; + +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; + +/** + * A wrapper for the CTRE CANCoder sensor. The encoder is automatically + * configured with the appropriate values for use with SwerveIO. + * + * @author Jordan Bancino + * @version 5.0.2 + * @since 5.0.2 + */ +@Loggable +public class PhoenixCANCoder implements Encoder { + + private final CANCoder encoder; + + @Log(as = "rawValue", atLevel = LogLevel.IMPORTANT) + private double lastValue; + + /** + * Construct a new CANCoder at the given CAN ID. + * + * @param canId The CAN ID that this CANCoder is located at. + */ + public PhoenixCANCoder(int canId) { + encoder = new CANCoder(canId); + + /* Use absolute positioning 0 -> 360 */ + encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition); + encoder.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360); + encoder.setPositionToAbsolute(); + + /* + * Reverse the sensor direction so it counts clockwise is positive. + * (Default is counterclockwise is positive). + */ + encoder.configSensorDirection(true); + } + + @Override + public double get() { + lastValue = encoder.getAbsolutePosition(); + return lastValue; + } + + @Override + public double countsPerRevolution() { + return 360; + } + +} diff --git a/vendor/ctre/src/main/java/net/bancino/robotics/swerveio/encoder/PhoenixEncoder.java b/vendor/ctre/src/main/java/net/bancino/robotics/swerveio/encoder/PhoenixEncoder.java new file mode 100755 index 0000000..96f759c --- /dev/null +++ b/vendor/ctre/src/main/java/net/bancino/robotics/swerveio/encoder/PhoenixEncoder.java @@ -0,0 +1,115 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.encoder; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.BaseMotorController; + +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; + +/** + * A wrapper for a CTRE Motor Controller sensor. This class provides SwerveIO + * support for any encoder attached to a CTRE motor controller, whether it is + * the internal brushless motor encoder, or an external attached encoder. Refer + * to the CTRE Phoenix Java documentation for {@code FeedbackDevice} to see the + * supported sensors. + * + *

+ * Note that only the CTRE MagEncoder, Quad encoders, and analog encoders + * support {@link #countsPerRevolution()}. Support for others can be added if we + * know their counts per rev value. + *

+ * + * @author Jordan Bancino + * @version 5.0.1 + * @since 4.2.0 + */ +@Loggable +public class PhoenixEncoder implements Encoder { + + private final BaseMotorController motor; + private final FeedbackDevice feedbackDevice; + + @Log(as = "phoenixPidIdx") + private final int slot; + + @Log(as = "rawValue", atLevel = LogLevel.IMPORTANT) + private double lastValue; + + /** + * Create a new Phoenix encoder with a motor controller and a feedback mode. + * This constructor uses the default profile slot, which is 0. + * + * @param motor The motor controller to use. + * @param feedbackDevice The encoder mode to set on the motor at the given PID + * slot. + */ + public PhoenixEncoder(BaseMotorController motor, FeedbackDevice feedbackDevice) { + this(motor, feedbackDevice, 0); + } + + /** + * Create a new Phoenix encoder with a motor controller, the feedback mode, and + * a PID slot. + * + * @param motor The motor controller to use. + * @param feedbackDevice The encoder mode to set on the motor at the given PID + * slot. + * @param slot The PID slot to fetch encoder values from. + */ + public PhoenixEncoder(BaseMotorController motor, FeedbackDevice feedbackDevice, int slot) { + this.motor = motor; + + /* Set the sensor to use on this profile slot. */ + motor.configSelectedFeedbackSensor(feedbackDevice, slot, 0); + + /* + * Set the indicator light mode (Green == Increasing) + * + * This isn't required for the TalonFX using the integrated encoder, but is + * useful for other motors and controllers. + */ + motor.setSensorPhase(true); + + this.feedbackDevice = feedbackDevice; + this.slot = slot; + } + + @Override + public double get() { + lastValue = motor.getSelectedSensorPosition(slot); + return lastValue; + } + + @Override + public double countsPerRevolution() { + switch (feedbackDevice) { + case CTRE_MagEncoder_Absolute: + case CTRE_MagEncoder_Relative: + case QuadEncoder: + return 4096; + case IntegratedSensor: + return 2048; + case Analog: + return 1024; + default: + throw new IllegalArgumentException("Unknown sensor: " + feedbackDevice); + } + } +} diff --git a/vendor/kauai/build.gradle b/vendor/kauai/build.gradle new file mode 100755 index 0000000..8fed520 --- /dev/null +++ b/vendor/kauai/build.gradle @@ -0,0 +1,22 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +/* The KauaiLabs NavX API*/ +dependencies { + api 'com.kauailabs.navx.frc:navx-java:4.0.435' +} + +rootProject.javadocAddLinks(project, 'https://www.kauailabs.com/public_files/navx-mxp/apidocs/java/') diff --git a/vendor/kauai/src/javadoc/overview.html b/vendor/kauai/src/javadoc/overview.html new file mode 100755 index 0000000..5e1a026 --- /dev/null +++ b/vendor/kauai/src/javadoc/overview.html @@ -0,0 +1,12 @@ + + + + +

This API adds official SwerveIO support for Kauai Labs' + hardware. + You should have been directed here from the official SwerveIO documentation. If you weren't, + please go there now for instructions on getting this API set up for use. +

+ + + \ No newline at end of file diff --git a/vendor/kauai/src/main/java/net/bancino/robotics/swerveio/gyro/NavXGyro.java b/vendor/kauai/src/main/java/net/bancino/robotics/swerveio/gyro/NavXGyro.java new file mode 100755 index 0000000..18a45e8 --- /dev/null +++ b/vendor/kauai/src/main/java/net/bancino/robotics/swerveio/gyro/NavXGyro.java @@ -0,0 +1,102 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.gyro; + +import edu.wpi.first.wpilibj.I2C; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.SerialPort; +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; + +import com.kauailabs.navx.frc.AHRS; + +/** + * A Kauai Labs FRC NavX Gyro that implements the abstract gyro interface for + * use with the swerve drive. + * + * @author Jordan Bancino + * @version 4.1.1 + * @since 2.0.0 + */ +@Loggable +public class NavXGyro implements Gyro { + + private final AHRS gyro; + + @Log(atLevel = LogLevel.IMPORTANT) + private double angle; + + /** + * Create a gyro object on the given port. + * + * @param port The SPI port that the gyro is on. + */ + public NavXGyro(SPI.Port port) { + this(new AHRS(port)); + } + + /** + * Create a gyro object on the given port. + * + * @param port The I2C port that the gyro is on. + */ + public NavXGyro(I2C.Port port) { + this(new AHRS(port)); + } + + /** + * Create a gyro object on the given port. + * + * @param port The SerialPort port that the gyro is on. + */ + public NavXGyro(SerialPort.Port port) { + this(new AHRS(port)); + } + + /** + * Wrap an existing NavX AHRS object. This can be used if none of the other + * constructors suit your needs. Because AHRS offers a number of constructors, + * it doesn't make sense to duplicate them all here. This way, you can + * instantiate your AHRS and configure it however you want before adding it to + * SwerveIO. + * + * @param ahrs The AHRS object to use. + */ + public NavXGyro(AHRS ahrs) { + if (ahrs != null) { + gyro = ahrs; + } else { + throw new IllegalArgumentException("NavX AHRS cannot be null."); + } + } + + @Override + public double getAngle() { + double yaw = gyro.getYaw(); + if (yaw < 0) { + yaw += 360; + } + angle = yaw; + return angle; + } + + @Override + public void zero() { + gyro.zeroYaw(); + } +} diff --git a/vendor/rev/build.gradle b/vendor/rev/build.gradle new file mode 100755 index 0000000..5d719c8 --- /dev/null +++ b/vendor/rev/build.gradle @@ -0,0 +1,25 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +/* The RevRobotics REVLib API. */ +dependencies { + /* https://software-metadata.revrobotics.com/REVLib.json */ + def sparkMaxVersion = '2022.1.0' + api "com.revrobotics.frc:REVLib-java:${sparkMaxVersion}" + api "com.revrobotics.frc:REVLib-driver:${sparkMaxVersion}" +} + +rootProject.javadocAddLinks(project, 'https://codedocs.revrobotics.com/java/') diff --git a/vendor/rev/src/javadoc/overview.html b/vendor/rev/src/javadoc/overview.html new file mode 100755 index 0000000..3bb7e47 --- /dev/null +++ b/vendor/rev/src/javadoc/overview.html @@ -0,0 +1,12 @@ + + + + +

This API adds official SwerveIO support for RevRobotics' + hardware. + You should have been directed here from the official SwerveIO documentation. If you weren't, + please go there now for instructions on getting this API set up for use. +

+ + + \ No newline at end of file diff --git a/vendor/rev/src/main/java/net/bancino/robotics/swerveio/encoder/SparkMaxEncoder.java b/vendor/rev/src/main/java/net/bancino/robotics/swerveio/encoder/SparkMaxEncoder.java new file mode 100755 index 0000000..3514860 --- /dev/null +++ b/vendor/rev/src/main/java/net/bancino/robotics/swerveio/encoder/SparkMaxEncoder.java @@ -0,0 +1,106 @@ +/* + * SwerveIO - A versatile open-source FRC swerve drive library. + * + * Copyright (C) 2019-2022 Jordan Bancino + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published with this + * source code. You should have received a copy of the GNU General Public + * License with this program. If you did not, contact the distributors of + * this program and view it here: https://bancino.net/licenses/gpl-3.0.txt + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + */ +package net.bancino.robotics.swerveio.encoder; + +import com.revrobotics.*; + +import net.bancino.log.Log; +import net.bancino.log.LogLevel; +import net.bancino.log.Loggable; + +/** + * A wrapper for the RevRobotics SparkMax encoder interface. This class provides + * SwerveIO support for any encoder attached to the Spark Max motor controller, + * whether it is the internal brushless motor encoder, an alternate encoder, or + * an analog encoder. + * + * + * @author Jordan Bancino + * @version 8.0.0 + * @since 1.0.0 + */ +@Loggable +public class SparkMaxEncoder implements Encoder { + + private final Object encoder; + + @Log(as = "rawValue", atLevel = LogLevel.IMPORTANT) + private double lastValue; + + /** + * Create a new spark max encoder with a motor controller. This sets up the + * encoder to read from the internal NEO encoder data port. + * + * @param sparkMax The motor controller to use. + */ + public SparkMaxEncoder(CANSparkMax sparkMax) { + encoder = sparkMax.getEncoder(); + if (encoder == null) { + throw new UnsupportedOperationException("No internal encoder attached to Spark Max."); + } + } + + /** + * Create a new spark max encoder with a motor controller. This sets up the + * encode to read from an alternate encoder connected to a spark max. + * + * @param sparkMax The motor controller to use. + * @param encoderType The type of encoder that is connected. + * @param countsPerRev The counts per revolution of the encoder. + */ + public SparkMaxEncoder(CANSparkMax sparkMax, SparkMaxAlternateEncoder.Type encoderType, int countsPerRev) { + encoder = sparkMax.getAlternateEncoder(encoderType, countsPerRev); + if (encoder == null) { + throw new UnsupportedOperationException("No alternate encoder attached to Spark Max."); + } + } + + /** + * Create a new spark max encoder with a motor controller. This sets up the + * encoder to read from an analog encoder connected to the spark max. + * + * @param sparkMax The motor controller to use. + * @param analogMode The mode of the analog sensor. + */ + public SparkMaxEncoder(CANSparkMax sparkMax, SparkMaxAnalogSensor.Mode analogMode) { + encoder = sparkMax.getAnalog(analogMode); + if (encoder == null) { + throw new UnsupportedOperationException("No analog encoder attached to Spark Max."); + } + } + + @Override + public double get() { + if (encoder instanceof RelativeEncoder) { + lastValue = ((RelativeEncoder) encoder).getPosition(); + } else if (encoder instanceof SparkMaxAnalogSensor) { + lastValue = ((SparkMaxAnalogSensor) encoder).getPosition(); + } else { + throw new IllegalStateException("Something happened and I don't know what."); + } + return lastValue; + } + + @Override + public double countsPerRevolution() { + if (encoder instanceof RelativeEncoder) { + return ((RelativeEncoder) encoder).getCountsPerRevolution(); + } else { + throw new IllegalArgumentException("SparkMaxAnalogSensor does not support counts per revolution."); + } + } +} \ No newline at end of file