commit e594f42c698ddac46d25f387c17ec2952d4f6e1c Author: Jordan Bancino Date: Tue Jan 2 18:14:26 2024 -0500 Initial import from CVS. 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 0000000..cc4fdc2 Binary files /dev/null and b/gradle/wrapper/gradle-wrapper.jar differ 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