From db79f69a60aeab769985d2fbdfef21cf9d7092a7 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Mon, 23 Sep 2024 11:46:53 -0700 Subject: [PATCH 01/57] Getting some basics in place modified: .gitignore modified: LICENSE new file: WPILib-License.md --- .gitignore | 154 +++++++- LICENSE | 875 +++++++++++++++++++++++++++++++++++----------- WPILib-License.md | 24 ++ 3 files changed, 851 insertions(+), 202 deletions(-) create mode 100644 WPILib-License.md diff --git a/.gitignore b/.gitignore index 524f0963..b8b5f586 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,41 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### # Compiled class file *.class @@ -21,4 +59,118 @@ # virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml hs_err_pid* -replay_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +*.code-workspace + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +simgui*.json + +# Version file +src/main/java/frc/robot/BuildConstants.java \ No newline at end of file diff --git a/LICENSE b/LICENSE index 261eeb9e..f288702d 100644 --- a/LICENSE +++ b/LICENSE @@ -1,201 +1,674 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - 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 - - http://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. + 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 + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + 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. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + 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/WPILib-License.md b/WPILib-License.md new file mode 100644 index 00000000..645e5425 --- /dev/null +++ b/WPILib-License.md @@ -0,0 +1,24 @@ +Copyright (c) 2009-2024 FIRST and other WPILib contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of FIRST, WPILib, nor the names of other WPILib + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. From 0e792ad55d279979ce44e6a073ba2eef3272fc72 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Mon, 23 Sep 2024 12:44:17 -0700 Subject: [PATCH 02/57] Starting to fill in the skeleton new file: .vscode/launch.json new file: .vscode/settings.json new file: .wpilib/wpilib_preferences.json new file: AdvantageKit-License.md new file: build.gradle new file: gradle/wrapper/gradle-wrapper.jar new file: gradle/wrapper/gradle-wrapper.properties new file: gradlew new file: gradlew.bat new file: settings.gradle new file: src/main/deploy/README.md new file: src/main/java/frc/robot/Constants.java new file: src/main/java/frc/robot/FieldConstants2024.java new file: src/main/java/frc/robot/Main.java new file: src/main/java/frc/robot/Robot.java new file: src/main/java/frc/robot/RobotContainer.java new file: src/main/java/frc/robot/util/Alert.java new file: src/main/java/frc/robot/util/AllianceFlipUtil.java new file: src/main/java/frc/robot/util/GeomUtil.java new file: src/main/java/frc/robot/util/LocalADStarAK.java new file: src/main/java/frc/robot/util/LoggedTunableNumber.java new file: src/main/java/frc/robot/util/OverrideSwitches.java new file: src/main/java/frc/robot/util/VirtualSubsystem.java new file: src/main/java/frc/robot/util/swerve/ModuleLimits.java new file: vendordeps/AdvantageKit.json new file: vendordeps/PathplannerLib.json new file: vendordeps/Phoenix5.json new file: vendordeps/Phoenix6.json new file: vendordeps/REVLib.json new file: vendordeps/WPILibNewCommands.json new file: vendordeps/photonlib.json new file: vendordeps/yagsl.json --- .vscode/launch.json | 17 + .vscode/settings.json | 32 + .wpilib/wpilib_preferences.json | 6 + AdvantageKit-License.md | 674 ++++++++++++++++++ build.gradle | 188 +++++ gradle/wrapper/gradle-wrapper.jar | Bin 0 -> 43462 bytes gradle/wrapper/gradle-wrapper.properties | 7 + gradlew | 249 +++++++ gradlew.bat | 92 +++ settings.gradle | 30 + src/main/deploy/README.md | 3 + src/main/java/frc/robot/Constants.java | 79 ++ .../java/frc/robot/FieldConstants2024.java | 201 ++++++ src/main/java/frc/robot/Main.java | 34 + src/main/java/frc/robot/Robot.java | 167 +++++ src/main/java/frc/robot/RobotContainer.java | 20 + src/main/java/frc/robot/util/Alert.java | 155 ++++ .../java/frc/robot/util/AllianceFlipUtil.java | 105 +++ src/main/java/frc/robot/util/GeomUtil.java | 171 +++++ .../java/frc/robot/util/LocalADStarAK.java | 153 ++++ .../frc/robot/util/LoggedTunableNumber.java | 131 ++++ .../java/frc/robot/util/OverrideSwitches.java | 92 +++ .../java/frc/robot/util/VirtualSubsystem.java | 38 + .../frc/robot/util/swerve/ModuleLimits.java | 13 + vendordeps/AdvantageKit.json | 42 ++ vendordeps/PathplannerLib.json | 38 + vendordeps/Phoenix5.json | 151 ++++ vendordeps/Phoenix6.json | 339 +++++++++ vendordeps/REVLib.json | 74 ++ vendordeps/WPILibNewCommands.json | 38 + vendordeps/photonlib.json | 57 ++ vendordeps/yagsl.json | 20 + 32 files changed, 3416 insertions(+) create mode 100644 .vscode/launch.json create mode 100644 .vscode/settings.json create mode 100644 .wpilib/wpilib_preferences.json create mode 100644 AdvantageKit-License.md create mode 100644 build.gradle create mode 100644 gradle/wrapper/gradle-wrapper.jar create mode 100644 gradle/wrapper/gradle-wrapper.properties create mode 100755 gradlew create mode 100644 gradlew.bat create mode 100644 settings.gradle create mode 100644 src/main/deploy/README.md create mode 100644 src/main/java/frc/robot/Constants.java create mode 100644 src/main/java/frc/robot/FieldConstants2024.java create mode 100644 src/main/java/frc/robot/Main.java create mode 100644 src/main/java/frc/robot/Robot.java create mode 100644 src/main/java/frc/robot/RobotContainer.java create mode 100644 src/main/java/frc/robot/util/Alert.java create mode 100644 src/main/java/frc/robot/util/AllianceFlipUtil.java create mode 100644 src/main/java/frc/robot/util/GeomUtil.java create mode 100644 src/main/java/frc/robot/util/LocalADStarAK.java create mode 100644 src/main/java/frc/robot/util/LoggedTunableNumber.java create mode 100644 src/main/java/frc/robot/util/OverrideSwitches.java create mode 100644 src/main/java/frc/robot/util/VirtualSubsystem.java create mode 100644 src/main/java/frc/robot/util/swerve/ModuleLimits.java create mode 100644 vendordeps/AdvantageKit.json create mode 100644 vendordeps/PathplannerLib.json create mode 100644 vendordeps/Phoenix5.json create mode 100644 vendordeps/Phoenix6.json create mode 100644 vendordeps/REVLib.json create mode 100644 vendordeps/WPILibNewCommands.json create mode 100644 vendordeps/photonlib.json create mode 100644 vendordeps/yagsl.json diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 00000000..b8c19206 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,17 @@ +{ + "version": "0.2.0", + "configurations": [ + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..a2e5322d --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,32 @@ +{ + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ + "-Djava.library.path=${workspaceFolder}/build/jni/release" + ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + null + ], + "java.test.defaultConfig": "WPIlibUnitTests" +} diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 00000000..319f962f --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2024", + "teamNumber": 2486 +} diff --git a/AdvantageKit-License.md b/AdvantageKit-License.md new file mode 100644 index 00000000..f288702d --- /dev/null +++ b/AdvantageKit-License.md @@ -0,0 +1,674 @@ + 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 + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + 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. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + 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/build.gradle b/build.gradle new file mode 100644 index 00000000..4c01a983 --- /dev/null +++ b/build.gradle @@ -0,0 +1,188 @@ +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2024.3.2" + id "com.peterabeles.gversion" version "1.10" + id "com.diffplug.spotless" version "6.12.0" + id "io.freefair.lombok" version "8.4" + id "com.google.protobuf" version "0.9.4" +} + +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} + +def ROBOT_MAIN_CLASS = "frc.robot.Main" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = project.frc.getTeamNumber() + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcJava + +// Set to true to use debug for JNI. +wpi.java.debugJni = false + +// Set this to true to enable desktop support. +def includeDesktopSupport = true + +// Configuration for AdvantageKit +repositories { + maven { + url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") + credentials { + username = "Mechanical-Advantage-Bot" + password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" + } + } + mavenLocal() +} + +configurations.all { + exclude group: "edu.wpi.first.wpilibj" +} + +task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { + mainClass = "org.littletonrobotics.junction.CheckInstall" + classpath = sourceSets.main.runtimeClasspath +} +compileJava.finalizedBy checkAkitInstall + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 4. +dependencies { + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() + + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() + + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() + + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" +} + +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} + +// Simulation configuration (e.g. environment variables). +// +// The sim GUI is *disabled* by default to support running +// AdvantageKit log replay from the command line. Set the +// value to "true" to enable the sim GUI by default (this +// is the standard WPILib behavior). +wpi.sim.addGui().defaultEnabled = false +wpi.sim.addDriverstation() + +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') +// in order to make them all available at runtime. Also adding the manifest so WPILib +// knows where to look for our Robot Class. +jar { + from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from sourceSets.main.allSource + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + duplicatesStrategy = DuplicatesStrategy.INCLUDE +} + +// Configure jar and deploy tasks +deployArtifact.jarTask = jar +wpi.java.configureExecutableTasks(jar) +wpi.java.configureTestTasks(test) + +// Configure string concat to always inline compile +tasks.withType(JavaCompile) { + options.compilerArgs.add '-XDstringConcat=inline' +} + +// Create version file +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "src/main/java/" + classPackage = "frc.robot" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/Phoenix" + indent = " " +} + +// Spotless formatting +project.compileJava.dependsOn(spotlessApply) +spotless { + java { + target fileTree(".") { + include "**/*.java" + exclude "**/build/**", "**/build-*/**" + } + toggleOffOn() + googleJavaFormat() + removeUnusedImports() + trimTrailingWhitespace() + endWithNewline() + } + groovyGradle { + target fileTree(".") { + include "**/*.gradle" + exclude "**/build/**", "**/build-*/**" + } + greclipse() + indentWithSpaces(4) + trimTrailingWhitespace() + endWithNewline() + } + json { + target fileTree(".") { + include "**/*.json" + exclude "**/build/**", "**/build-*/**" + } + gson().indentWithSpaces(2) + } + format "misc", { + target fileTree(".") { + include "**/*.md", "**/.gitignore" + exclude "**/build/**", "**/build-*/**" + } + trimTrailingWhitespace() + indentWithSpaces(2) + endWithNewline() + } +} diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000000000000000000000000000000000000..d64cd4917707c1f8861d8cb53dd15194d4248596 GIT binary patch literal 43462 zcma&NWl&^owk(X(xVyW%ySuwf;qI=D6|RlDJ2cR^yEKh!@I- zp9QeisK*rlxC>+~7Dk4IxIRsKBHqdR9b3+fyL=ynHmIDe&|>O*VlvO+%z5;9Z$|DJ zb4dO}-R=MKr^6EKJiOrJdLnCJn>np?~vU-1sSFgPu;pthGwf}bG z(1db%xwr#x)r+`4AGu$j7~u2MpVs3VpLp|mx&;>`0p0vH6kF+D2CY0fVdQOZ@h;A` z{infNyvmFUiu*XG}RNMNwXrbec_*a3N=2zJ|Wh5z* z5rAX$JJR{#zP>KY**>xHTuw?|-Rg|o24V)74HcfVT;WtQHXlE+_4iPE8QE#DUm%x0 zEKr75ur~W%w#-My3Tj`hH6EuEW+8K-^5P62$7Sc5OK+22qj&Pd1;)1#4tKihi=~8C zHiQSst0cpri6%OeaR`PY>HH_;CPaRNty%WTm4{wDK8V6gCZlG@U3$~JQZ;HPvDJcT1V{ z?>H@13MJcCNe#5z+MecYNi@VT5|&UiN1D4ATT+%M+h4c$t;C#UAs3O_q=GxK0}8%8 z8J(_M9bayxN}69ex4dzM_P3oh@ZGREjVvn%%r7=xjkqxJP4kj}5tlf;QosR=%4L5y zWhgejO=vao5oX%mOHbhJ8V+SG&K5dABn6!WiKl{|oPkq(9z8l&Mm%(=qGcFzI=eLu zWc_oCLyf;hVlB@dnwY98?75B20=n$>u3b|NB28H0u-6Rpl((%KWEBOfElVWJx+5yg z#SGqwza7f}$z;n~g%4HDU{;V{gXIhft*q2=4zSezGK~nBgu9-Q*rZ#2f=Q}i2|qOp z!!y4p)4o=LVUNhlkp#JL{tfkhXNbB=Ox>M=n6soptJw-IDI|_$is2w}(XY>a=H52d z3zE$tjPUhWWS+5h=KVH&uqQS=$v3nRs&p$%11b%5qtF}S2#Pc`IiyBIF4%A!;AVoI zXU8-Rpv!DQNcF~(qQnyyMy=-AN~U>#&X1j5BLDP{?K!%h!;hfJI>$mdLSvktEr*89 zdJHvby^$xEX0^l9g$xW-d?J;L0#(`UT~zpL&*cEh$L|HPAu=P8`OQZV!-}l`noSp_ zQ-1$q$R-gDL)?6YaM!=8H=QGW$NT2SeZlb8PKJdc=F-cT@j7Xags+Pr*jPtlHFnf- zh?q<6;)27IdPc^Wdy-mX%2s84C1xZq9Xms+==F4);O`VUASmu3(RlgE#0+#giLh-& zcxm3_e}n4{%|X zJp{G_j+%`j_q5}k{eW&TlP}J2wtZ2^<^E(O)4OQX8FDp6RJq!F{(6eHWSD3=f~(h} zJXCf7=r<16X{pHkm%yzYI_=VDP&9bmI1*)YXZeB}F? z(%QsB5fo*FUZxK$oX~X^69;x~j7ms8xlzpt-T15e9}$4T-pC z6PFg@;B-j|Ywajpe4~bk#S6(fO^|mm1hKOPfA%8-_iGCfICE|=P_~e;Wz6my&)h_~ zkv&_xSAw7AZ%ThYF(4jADW4vg=oEdJGVOs>FqamoL3Np8>?!W#!R-0%2Bg4h?kz5I zKV-rKN2n(vUL%D<4oj@|`eJ>0i#TmYBtYmfla;c!ATW%;xGQ0*TW@PTlGG><@dxUI zg>+3SiGdZ%?5N=8uoLA|$4isK$aJ%i{hECP$bK{J#0W2gQ3YEa zZQ50Stn6hqdfxJ*9#NuSLwKFCUGk@c=(igyVL;;2^wi4o30YXSIb2g_ud$ zgpCr@H0qWtk2hK8Q|&wx)}4+hTYlf;$a4#oUM=V@Cw#!$(nOFFpZ;0lc!qd=c$S}Z zGGI-0jg~S~cgVT=4Vo)b)|4phjStD49*EqC)IPwyeKBLcN;Wu@Aeph;emROAwJ-0< z_#>wVm$)ygH|qyxZaet&(Vf%pVdnvKWJn9`%DAxj3ot;v>S$I}jJ$FLBF*~iZ!ZXE zkvui&p}fI0Y=IDX)mm0@tAd|fEHl~J&K}ZX(Mm3cm1UAuwJ42+AO5@HwYfDH7ipIc zmI;1J;J@+aCNG1M`Btf>YT>~c&3j~Qi@Py5JT6;zjx$cvOQW@3oQ>|}GH?TW-E z1R;q^QFjm5W~7f}c3Ww|awg1BAJ^slEV~Pk`Kd`PS$7;SqJZNj->it4DW2l15}xP6 zoCl$kyEF%yJni0(L!Z&14m!1urXh6Btj_5JYt1{#+H8w?5QI%% zo-$KYWNMJVH?Hh@1n7OSu~QhSswL8x0=$<8QG_zepi_`y_79=nK=_ZP_`Em2UI*tyQoB+r{1QYZCpb?2OrgUw#oRH$?^Tj!Req>XiE#~B|~ z+%HB;=ic+R@px4Ld8mwpY;W^A%8%l8$@B@1m5n`TlKI6bz2mp*^^^1mK$COW$HOfp zUGTz-cN9?BGEp}5A!mDFjaiWa2_J2Iq8qj0mXzk; z66JBKRP{p%wN7XobR0YjhAuW9T1Gw3FDvR5dWJ8ElNYF94eF3ebu+QwKjtvVu4L zI9ip#mQ@4uqVdkl-TUQMb^XBJVLW(-$s;Nq;@5gr4`UfLgF$adIhd?rHOa%D);whv z=;krPp~@I+-Z|r#s3yCH+c1US?dnm+C*)r{m+86sTJusLdNu^sqLrfWed^ndHXH`m zd3#cOe3>w-ga(Dus_^ppG9AC>Iq{y%%CK+Cro_sqLCs{VLuK=dev>OL1dis4(PQ5R zcz)>DjEkfV+MO;~>VUlYF00SgfUo~@(&9$Iy2|G0T9BSP?&T22>K46D zL*~j#yJ?)^*%J3!16f)@Y2Z^kS*BzwfAQ7K96rFRIh>#$*$_Io;z>ux@}G98!fWR@ zGTFxv4r~v)Gsd|pF91*-eaZ3Qw1MH$K^7JhWIdX%o$2kCbvGDXy)a?@8T&1dY4`;L z4Kn+f%SSFWE_rpEpL9bnlmYq`D!6F%di<&Hh=+!VI~j)2mfil03T#jJ_s?}VV0_hp z7T9bWxc>Jm2Z0WMU?`Z$xE74Gu~%s{mW!d4uvKCx@WD+gPUQ zV0vQS(Ig++z=EHN)BR44*EDSWIyT~R4$FcF*VEY*8@l=218Q05D2$|fXKFhRgBIEE zdDFB}1dKkoO^7}{5crKX!p?dZWNz$m>1icsXG2N+((x0OIST9Zo^DW_tytvlwXGpn zs8?pJXjEG;T@qrZi%#h93?FP$!&P4JA(&H61tqQi=opRzNpm zkrG}$^t9&XduK*Qa1?355wd8G2CI6QEh@Ua>AsD;7oRUNLPb76m4HG3K?)wF~IyS3`fXuNM>${?wmB zpVz;?6_(Fiadfd{vUCBM*_kt$+F3J+IojI;9L(gc9n3{sEZyzR9o!_mOwFC#tQ{Q~ zP3-`#uK#tP3Q7~Q;4H|wjZHO8h7e4IuBxl&vz2w~D8)w=Wtg31zpZhz%+kzSzL*dV zwp@{WU4i;hJ7c2f1O;7Mz6qRKeASoIv0_bV=i@NMG*l<#+;INk-^`5w@}Dj~;k=|}qM1vq_P z|GpBGe_IKq|LNy9SJhKOQ$c=5L{Dv|Q_lZl=-ky*BFBJLW9&y_C|!vyM~rQx=!vun z?rZJQB5t}Dctmui5i31C_;_}CEn}_W%>oSXtt>@kE1=JW*4*v4tPp;O6 zmAk{)m!)}34pTWg8{i>($%NQ(Tl;QC@J@FfBoc%Gr&m560^kgSfodAFrIjF}aIw)X zoXZ`@IsMkc8_=w%-7`D6Y4e*CG8k%Ud=GXhsTR50jUnm+R*0A(O3UKFg0`K;qp1bl z7``HN=?39ic_kR|^R^~w-*pa?Vj#7|e9F1iRx{GN2?wK!xR1GW!qa=~pjJb-#u1K8 zeR?Y2i-pt}yJq;SCiVHODIvQJX|ZJaT8nO+(?HXbLefulKKgM^B(UIO1r+S=7;kLJ zcH}1J=Px2jsh3Tec&v8Jcbng8;V-`#*UHt?hB(pmOipKwf3Lz8rG$heEB30Sg*2rx zV<|KN86$soN(I!BwO`1n^^uF2*x&vJ$2d$>+`(romzHP|)K_KkO6Hc>_dwMW-M(#S zK(~SiXT1@fvc#U+?|?PniDRm01)f^#55;nhM|wi?oG>yBsa?~?^xTU|fX-R(sTA+5 zaq}-8Tx7zrOy#3*JLIIVsBmHYLdD}!0NP!+ITW+Thn0)8SS!$@)HXwB3tY!fMxc#1 zMp3H?q3eD?u&Njx4;KQ5G>32+GRp1Ee5qMO0lZjaRRu&{W<&~DoJNGkcYF<5(Ab+J zgO>VhBl{okDPn78<%&e2mR{jwVCz5Og;*Z;;3%VvoGo_;HaGLWYF7q#jDX=Z#Ml`H z858YVV$%J|e<1n`%6Vsvq7GmnAV0wW4$5qQ3uR@1i>tW{xrl|ExywIc?fNgYlA?C5 zh$ezAFb5{rQu6i7BSS5*J-|9DQ{6^BVQ{b*lq`xS@RyrsJN?-t=MTMPY;WYeKBCNg z^2|pN!Q^WPJuuO4!|P@jzt&tY1Y8d%FNK5xK(!@`jO2aEA*4 zkO6b|UVBipci?){-Ke=+1;mGlND8)6+P;8sq}UXw2hn;fc7nM>g}GSMWu&v&fqh

iViYT=fZ(|3Ox^$aWPp4a8h24tD<|8-!aK0lHgL$N7Efw}J zVIB!7=T$U`ao1?upi5V4Et*-lTG0XvExbf!ya{cua==$WJyVG(CmA6Of*8E@DSE%L z`V^$qz&RU$7G5mg;8;=#`@rRG`-uS18$0WPN@!v2d{H2sOqP|!(cQ@ zUHo!d>>yFArLPf1q`uBvY32miqShLT1B@gDL4XoVTK&@owOoD)OIHXrYK-a1d$B{v zF^}8D3Y^g%^cnvScOSJR5QNH+BI%d|;J;wWM3~l>${fb8DNPg)wrf|GBP8p%LNGN# z3EaIiItgwtGgT&iYCFy9-LG}bMI|4LdmmJt@V@% zb6B)1kc=T)(|L@0;wr<>=?r04N;E&ef+7C^`wPWtyQe(*pD1pI_&XHy|0gIGHMekd zF_*M4yi6J&Z4LQj65)S zXwdM{SwUo%3SbPwFsHgqF@V|6afT|R6?&S;lw=8% z3}@9B=#JI3@B*#4s!O))~z zc>2_4Q_#&+5V`GFd?88^;c1i7;Vv_I*qt!_Yx*n=;rj!82rrR2rQ8u5(Ejlo{15P% zs~!{%XJ>FmJ})H^I9bn^Re&38H{xA!0l3^89k(oU;bZWXM@kn$#aoS&Y4l^-WEn-fH39Jb9lA%s*WsKJQl?n9B7_~P z-XM&WL7Z!PcoF6_D>V@$CvUIEy=+Z&0kt{szMk=f1|M+r*a43^$$B^MidrT0J;RI` z(?f!O<8UZkm$_Ny$Hth1J#^4ni+im8M9mr&k|3cIgwvjAgjH z8`N&h25xV#v*d$qBX5jkI|xOhQn!>IYZK7l5#^P4M&twe9&Ey@@GxYMxBZq2e7?`q z$~Szs0!g{2fGcp9PZEt|rdQ6bhAgpcLHPz?f-vB?$dc*!9OL?Q8mn7->bFD2Si60* z!O%y)fCdMSV|lkF9w%x~J*A&srMyYY3{=&$}H zGQ4VG_?$2X(0|vT0{=;W$~icCI{b6W{B!Q8xdGhF|D{25G_5_+%s(46lhvNLkik~R z>nr(&C#5wwOzJZQo9m|U<;&Wk!_#q|V>fsmj1g<6%hB{jGoNUPjgJslld>xmODzGjYc?7JSuA?A_QzjDw5AsRgi@Y|Z0{F{!1=!NES-#*f^s4l0Hu zz468))2IY5dmD9pa*(yT5{EyP^G>@ZWumealS-*WeRcZ}B%gxq{MiJ|RyX-^C1V=0 z@iKdrGi1jTe8Ya^x7yyH$kBNvM4R~`fbPq$BzHum-3Zo8C6=KW@||>zsA8-Y9uV5V z#oq-f5L5}V<&wF4@X@<3^C%ptp6+Ce)~hGl`kwj)bsAjmo_GU^r940Z-|`<)oGnh7 zFF0Tde3>ui?8Yj{sF-Z@)yQd~CGZ*w-6p2U<8}JO-sRsVI5dBji`01W8A&3$?}lxBaC&vn0E$c5tW* zX>5(zzZ=qn&!J~KdsPl;P@bmA-Pr8T*)eh_+Dv5=Ma|XSle6t(k8qcgNyar{*ReQ8 zTXwi=8vr>!3Ywr+BhggHDw8ke==NTQVMCK`$69fhzEFB*4+H9LIvdt-#IbhZvpS}} zO3lz;P?zr0*0$%-Rq_y^k(?I{Mk}h@w}cZpMUp|ucs55bcloL2)($u%mXQw({Wzc~ z;6nu5MkjP)0C(@%6Q_I_vsWrfhl7Zpoxw#WoE~r&GOSCz;_ro6i(^hM>I$8y>`!wW z*U^@?B!MMmb89I}2(hcE4zN2G^kwyWCZp5JG>$Ez7zP~D=J^LMjSM)27_0B_X^C(M z`fFT+%DcKlu?^)FCK>QzSnV%IsXVcUFhFdBP!6~se&xxrIxsvySAWu++IrH;FbcY$ z2DWTvSBRfLwdhr0nMx+URA$j3i7_*6BWv#DXfym?ZRDcX9C?cY9sD3q)uBDR3uWg= z(lUIzB)G$Hr!){>E{s4Dew+tb9kvToZp-1&c?y2wn@Z~(VBhqz`cB;{E4(P3N2*nJ z_>~g@;UF2iG{Kt(<1PyePTKahF8<)pozZ*xH~U-kfoAayCwJViIrnqwqO}7{0pHw$ zs2Kx?s#vQr7XZ264>5RNKSL8|Ty^=PsIx^}QqOOcfpGUU4tRkUc|kc7-!Ae6!+B{o~7nFpm3|G5^=0#Bnm6`V}oSQlrX(u%OWnC zoLPy&Q;1Jui&7ST0~#+}I^&?vcE*t47~Xq#YwvA^6^} z`WkC)$AkNub|t@S!$8CBlwbV~?yp&@9h{D|3z-vJXgzRC5^nYm+PyPcgRzAnEi6Q^gslXYRv4nycsy-SJu?lMps-? zV`U*#WnFsdPLL)Q$AmD|0`UaC4ND07+&UmOu!eHruzV|OUox<+Jl|Mr@6~C`T@P%s zW7sgXLF2SSe9Fl^O(I*{9wsFSYb2l%-;&Pi^dpv!{)C3d0AlNY6!4fgmSgj_wQ*7Am7&$z;Jg&wgR-Ih;lUvWS|KTSg!&s_E9_bXBkZvGiC6bFKDWZxsD$*NZ#_8bl zG1P-#@?OQzED7@jlMJTH@V!6k;W>auvft)}g zhoV{7$q=*;=l{O>Q4a@ ziMjf_u*o^PsO)#BjC%0^h>Xp@;5$p{JSYDt)zbb}s{Kbt!T*I@Pk@X0zds6wsefuU zW$XY%yyRGC94=6mf?x+bbA5CDQ2AgW1T-jVAJbm7K(gp+;v6E0WI#kuACgV$r}6L? zd|Tj?^%^*N&b>Dd{Wr$FS2qI#Ucs1yd4N+RBUQiSZGujH`#I)mG&VKoDh=KKFl4=G z&MagXl6*<)$6P}*Tiebpz5L=oMaPrN+caUXRJ`D?=K9!e0f{@D&cZLKN?iNP@X0aF zE(^pl+;*T5qt?1jRC=5PMgV!XNITRLS_=9{CJExaQj;lt!&pdzpK?8p>%Mb+D z?yO*uSung=-`QQ@yX@Hyd4@CI^r{2oiu`%^bNkz+Nkk!IunjwNC|WcqvX~k=><-I3 zDQdbdb|!v+Iz01$w@aMl!R)koD77Xp;eZwzSl-AT zr@Vu{=xvgfq9akRrrM)}=!=xcs+U1JO}{t(avgz`6RqiiX<|hGG1pmop8k6Q+G_mv zJv|RfDheUp2L3=^C=4aCBMBn0aRCU(DQwX-W(RkRwmLeuJYF<0urcaf(=7)JPg<3P zQs!~G)9CT18o!J4{zX{_e}4eS)U-E)0FAt}wEI(c0%HkxgggW;(1E=>J17_hsH^sP z%lT0LGgbUXHx-K*CI-MCrP66UP0PvGqM$MkeLyqHdbgP|_Cm!7te~b8p+e6sQ_3k| zVcwTh6d83ltdnR>D^)BYQpDKlLk3g0Hdcgz2}%qUs9~~Rie)A-BV1mS&naYai#xcZ z(d{8=-LVpTp}2*y)|gR~;qc7fp26}lPcLZ#=JpYcn3AT9(UIdOyg+d(P5T7D&*P}# zQCYplZO5|7+r19%9e`v^vfSS1sbX1c%=w1;oyruXB%Kl$ACgKQ6=qNWLsc=28xJjg zwvsI5-%SGU|3p>&zXVl^vVtQT3o-#$UT9LI@Npz~6=4!>mc431VRNN8od&Ul^+G_kHC`G=6WVWM z%9eWNyy(FTO|A+@x}Ou3CH)oi;t#7rAxdIXfNFwOj_@Y&TGz6P_sqiB`Q6Lxy|Q{`|fgmRG(k+!#b*M+Z9zFce)f-7;?Km5O=LHV9f9_87; zF7%R2B+$?@sH&&-$@tzaPYkw0;=i|;vWdI|Wl3q_Zu>l;XdIw2FjV=;Mq5t1Q0|f< zs08j54Bp`3RzqE=2enlkZxmX6OF+@|2<)A^RNQpBd6o@OXl+i)zO%D4iGiQNuXd+zIR{_lb96{lc~bxsBveIw6umhShTX+3@ZJ=YHh@ zWY3(d0azg;7oHn>H<>?4@*RQbi>SmM=JrHvIG(~BrvI)#W(EAeO6fS+}mxxcc+X~W6&YVl86W9WFSS}Vz-f9vS?XUDBk)3TcF z8V?$4Q)`uKFq>xT=)Y9mMFVTUk*NIA!0$?RP6Ig0TBmUFrq*Q-Agq~DzxjStQyJ({ zBeZ;o5qUUKg=4Hypm|}>>L=XKsZ!F$yNTDO)jt4H0gdQ5$f|d&bnVCMMXhNh)~mN z@_UV6D7MVlsWz+zM+inZZp&P4fj=tm6fX)SG5H>OsQf_I8c~uGCig$GzuwViK54bcgL;VN|FnyQl>Ed7(@>=8$a_UKIz|V6CeVSd2(P z0Uu>A8A+muM%HLFJQ9UZ5c)BSAv_zH#1f02x?h9C}@pN@6{>UiAp>({Fn(T9Q8B z^`zB;kJ5b`>%dLm+Ol}ty!3;8f1XDSVX0AUe5P#@I+FQ-`$(a;zNgz)4x5hz$Hfbg z!Q(z26wHLXko(1`;(BAOg_wShpX0ixfWq3ponndY+u%1gyX)_h=v1zR#V}#q{au6; z!3K=7fQwnRfg6FXtNQmP>`<;!N137paFS%y?;lb1@BEdbvQHYC{976l`cLqn;b8lp zIDY>~m{gDj(wfnK!lpW6pli)HyLEiUrNc%eXTil|F2s(AY+LW5hkKb>TQ3|Q4S9rr zpDs4uK_co6XPsn_z$LeS{K4jFF`2>U`tbgKdyDne`xmR<@6AA+_hPNKCOR-Zqv;xk zu5!HsBUb^!4uJ7v0RuH-7?l?}b=w5lzzXJ~gZcxRKOovSk@|#V+MuX%Y+=;14i*%{)_gSW9(#4%)AV#3__kac1|qUy!uyP{>?U#5wYNq}y$S9pCc zFc~4mgSC*G~j0u#qqp9 z${>3HV~@->GqEhr_Xwoxq?Hjn#=s2;i~g^&Hn|aDKpA>Oc%HlW(KA1?BXqpxB;Ydx)w;2z^MpjJ(Qi(X!$5RC z*P{~%JGDQqojV>2JbEeCE*OEu!$XJ>bWA9Oa_Hd;y)F%MhBRi*LPcdqR8X`NQ&1L# z5#9L*@qxrx8n}LfeB^J{%-?SU{FCwiWyHp682F+|pa+CQa3ZLzBqN1{)h4d6+vBbV zC#NEbQLC;}me3eeYnOG*nXOJZEU$xLZ1<1Y=7r0(-U0P6-AqwMAM`a(Ed#7vJkn6plb4eI4?2y3yOTGmmDQ!z9`wzbf z_OY#0@5=bnep;MV0X_;;SJJWEf^E6Bd^tVJ9znWx&Ks8t*B>AM@?;D4oWUGc z!H*`6d7Cxo6VuyS4Eye&L1ZRhrRmN6Lr`{NL(wDbif|y&z)JN>Fl5#Wi&mMIr5i;x zBx}3YfF>>8EC(fYnmpu~)CYHuHCyr5*`ECap%t@y=jD>!_%3iiE|LN$mK9>- zHdtpy8fGZtkZF?%TW~29JIAfi2jZT8>OA7=h;8T{{k?c2`nCEx9$r zS+*&vt~2o^^J+}RDG@+9&M^K*z4p{5#IEVbz`1%`m5c2};aGt=V?~vIM}ZdPECDI)47|CWBCfDWUbxBCnmYivQ*0Nu_xb*C>~C9(VjHM zxe<*D<#dQ8TlpMX2c@M<9$w!RP$hpG4cs%AI){jp*Sj|*`m)5(Bw*A0$*i-(CA5#%>a)$+jI2C9r6|(>J8InryENI z$NohnxDUB;wAYDwrb*!N3noBTKPpPN}~09SEL18tkG zxgz(RYU_;DPT{l?Q$+eaZaxnsWCA^ds^0PVRkIM%bOd|G2IEBBiz{&^JtNsODs;5z zICt_Zj8wo^KT$7Bg4H+y!Df#3mbl%%?|EXe!&(Vmac1DJ*y~3+kRKAD=Ovde4^^%~ zw<9av18HLyrf*_>Slp;^i`Uy~`mvBjZ|?Ad63yQa#YK`4+c6;pW4?XIY9G1(Xh9WO8{F-Aju+nS9Vmv=$Ac0ienZ+p9*O%NG zMZKy5?%Z6TAJTE?o5vEr0r>f>hb#2w2U3DL64*au_@P!J!TL`oH2r*{>ffu6|A7tv zL4juf$DZ1MW5ZPsG!5)`k8d8c$J$o;%EIL0va9&GzWvkS%ZsGb#S(?{!UFOZ9<$a| zY|a+5kmD5N&{vRqkgY>aHsBT&`rg|&kezoD)gP0fsNYHsO#TRc_$n6Lf1Z{?+DLziXlHrq4sf(!>O{?Tj;Eh@%)+nRE_2VxbN&&%%caU#JDU%vL3}Cb zsb4AazPI{>8H&d=jUaZDS$-0^AxE@utGs;-Ez_F(qC9T=UZX=>ok2k2 ziTn{K?y~a5reD2A)P${NoI^>JXn>`IeArow(41c-Wm~)wiryEP(OS{YXWi7;%dG9v zI?mwu1MxD{yp_rrk!j^cKM)dc4@p4Ezyo%lRN|XyD}}>v=Xoib0gOcdXrQ^*61HNj z=NP|pd>@yfvr-=m{8$3A8TQGMTE7g=z!%yt`8`Bk-0MMwW~h^++;qyUP!J~ykh1GO z(FZ59xuFR$(WE;F@UUyE@Sp>`aVNjyj=Ty>_Vo}xf`e7`F;j-IgL5`1~-#70$9_=uBMq!2&1l zomRgpD58@)YYfvLtPW}{C5B35R;ZVvB<<#)x%srmc_S=A7F@DW8>QOEGwD6suhwCg z>Pa+YyULhmw%BA*4yjDp|2{!T98~<6Yfd(wo1mQ!KWwq0eg+6)o1>W~f~kL<-S+P@$wx*zeI|1t7z#Sxr5 zt6w+;YblPQNplq4Z#T$GLX#j6yldXAqj>4gAnnWtBICUnA&-dtnlh=t0Ho_vEKwV` z)DlJi#!@nkYV#$!)@>udAU*hF?V`2$Hf=V&6PP_|r#Iv*J$9)pF@X3`k;5})9^o4y z&)~?EjX5yX12O(BsFy-l6}nYeuKkiq`u9145&3Ssg^y{5G3Pse z9w(YVa0)N-fLaBq1`P!_#>SS(8fh_5!f{UrgZ~uEdeMJIz7DzI5!NHHqQtm~#CPij z?=N|J>nPR6_sL7!f4hD_|KH`vf8(Wpnj-(gPWH+ZvID}%?~68SwhPTC3u1_cB`otq z)U?6qo!ZLi5b>*KnYHWW=3F!p%h1;h{L&(Q&{qY6)_qxNfbP6E3yYpW!EO+IW3?@J z);4>g4gnl^8klu7uA>eGF6rIGSynacogr)KUwE_R4E5Xzi*Qir@b-jy55-JPC8c~( zo!W8y9OGZ&`xmc8;=4-U9=h{vCqfCNzYirONmGbRQlR`WWlgnY+1wCXbMz&NT~9*| z6@FrzP!LX&{no2!Ln_3|I==_4`@}V?4a;YZKTdw;vT<+K+z=uWbW(&bXEaWJ^W8Td z-3&1bY^Z*oM<=M}LVt>_j+p=2Iu7pZmbXrhQ_k)ysE9yXKygFNw$5hwDn(M>H+e1&9BM5!|81vd%r%vEm zqxY3?F@fb6O#5UunwgAHR9jp_W2zZ}NGp2%mTW@(hz7$^+a`A?mb8|_G*GNMJ) zjqegXQio=i@AINre&%ofexAr95aop5C+0MZ0m-l=MeO8m3epm7U%vZB8+I+C*iNFM z#T3l`gknX;D$-`2XT^Cg*vrv=RH+P;_dfF++cP?B_msQI4j+lt&rX2)3GaJx%W*Nn zkML%D{z5tpHH=dksQ*gzc|}gzW;lwAbxoR07VNgS*-c3d&8J|;@3t^ zVUz*J*&r7DFRuFVDCJDK8V9NN5hvpgGjwx+5n)qa;YCKe8TKtdnh{I7NU9BCN!0dq zczrBk8pE{{@vJa9ywR@mq*J=v+PG;?fwqlJVhijG!3VmIKs>9T6r7MJpC)m!Tc#>g zMtVsU>wbwFJEfwZ{vB|ZlttNe83)$iz`~#8UJ^r)lJ@HA&G#}W&ZH*;k{=TavpjWE z7hdyLZPf*X%Gm}i`Y{OGeeu^~nB8=`{r#TUrM-`;1cBvEd#d!kPqIgYySYhN-*1;L z^byj%Yi}Gx)Wnkosi337BKs}+5H5dth1JA{Ir-JKN$7zC)*}hqeoD(WfaUDPT>0`- z(6sa0AoIqASwF`>hP}^|)a_j2s^PQn*qVC{Q}htR z5-)duBFXT_V56-+UohKXlq~^6uf!6sA#ttk1o~*QEy_Y-S$gAvq47J9Vtk$5oA$Ct zYhYJ@8{hsC^98${!#Ho?4y5MCa7iGnfz}b9jE~h%EAAv~Qxu)_rAV;^cygV~5r_~?l=B`zObj7S=H=~$W zPtI_m%g$`kL_fVUk9J@>EiBH zOO&jtn~&`hIFMS5S`g8w94R4H40mdNUH4W@@XQk1sr17b{@y|JB*G9z1|CrQjd+GX z6+KyURG3;!*BQrentw{B2R&@2&`2}n(z-2&X7#r!{yg@Soy}cRD~j zj9@UBW+N|4HW4AWapy4wfUI- zZ`gSL6DUlgj*f1hSOGXG0IVH8HxK?o2|3HZ;KW{K+yPAlxtb)NV_2AwJm|E)FRs&& z=c^e7bvUsztY|+f^k7NXs$o1EUq>cR7C0$UKi6IooHWlK_#?IWDkvywnzg&ThWo^? z2O_N{5X39#?eV9l)xI(>@!vSB{DLt*oY!K1R8}_?%+0^C{d9a%N4 zoxHVT1&Lm|uDX%$QrBun5e-F`HJ^T$ zmzv)p@4ZHd_w9!%Hf9UYNvGCw2TTTbrj9pl+T9%-_-}L(tES>Or-}Z4F*{##n3~L~TuxjirGuIY#H7{%$E${?p{Q01 zi6T`n;rbK1yIB9jmQNycD~yZq&mbIsFWHo|ZAChSFPQa<(%d8mGw*V3fh|yFoxOOiWJd(qvVb!Z$b88cg->N=qO*4k~6;R==|9ihg&riu#P~s4Oap9O7f%crSr^rljeIfXDEg>wi)&v*a%7zpz<9w z*r!3q9J|390x`Zk;g$&OeN&ctp)VKRpDSV@kU2Q>jtok($Y-*x8_$2piTxun81@vt z!Vj?COa0fg2RPXMSIo26T=~0d`{oGP*eV+$!0I<(4azk&Vj3SiG=Q!6mX0p$z7I}; z9BJUFgT-K9MQQ-0@Z=^7R<{bn2Fm48endsSs`V7_@%8?Bxkqv>BDoVcj?K#dV#uUP zL1ND~?D-|VGKe3Rw_7-Idpht>H6XRLh*U7epS6byiGvJpr%d}XwfusjH9g;Z98H`x zyde%%5mhGOiL4wljCaWCk-&uE4_OOccb9c!ZaWt4B(wYl!?vyzl%7n~QepN&eFUrw zFIOl9c({``6~QD+43*_tzP{f2x41h(?b43^y6=iwyB)2os5hBE!@YUS5?N_tXd=h( z)WE286Fbd>R4M^P{!G)f;h<3Q>Fipuy+d2q-)!RyTgt;wr$(?9ox3;q+{E*ZQHhOn;lM`cjnu9 zXa48ks-v(~b*;MAI<>YZH(^NV8vjb34beE<_cwKlJoR;k6lJNSP6v}uiyRD?|0w+X@o1ONrH8a$fCxXpf? z?$DL0)7|X}Oc%h^zrMKWc-NS9I0Utu@>*j}b@tJ=ixQSJ={4@854wzW@E>VSL+Y{i z#0b=WpbCZS>kUCO_iQz)LoE>P5LIG-hv9E+oG}DtlIDF>$tJ1aw9^LuhLEHt?BCj& z(O4I8v1s#HUi5A>nIS-JK{v!7dJx)^Yg%XjNmlkWAq2*cv#tHgz`Y(bETc6CuO1VkN^L-L3j_x<4NqYb5rzrLC-7uOv z!5e`GZt%B782C5-fGnn*GhDF$%(qP<74Z}3xx+{$4cYKy2ikxI7B2N+2r07DN;|-T->nU&!=Cm#rZt%O_5c&1Z%nlWq3TKAW0w zQqemZw_ue--2uKQsx+niCUou?HjD`xhEjjQd3%rrBi82crq*~#uA4+>vR<_S{~5ce z-2EIl?~s z1=GVL{NxP1N3%=AOaC}j_Fv=ur&THz zyO!d9kHq|c73kpq`$+t+8Bw7MgeR5~`d7ChYyGCBWSteTB>8WAU(NPYt2Dk`@#+}= zI4SvLlyk#pBgVigEe`?NG*vl7V6m+<}%FwPV=~PvvA)=#ths==DRTDEYh4V5}Cf$z@#;< zyWfLY_5sP$gc3LLl2x+Ii)#b2nhNXJ{R~vk`s5U7Nyu^3yFg&D%Txwj6QezMX`V(x z=C`{76*mNb!qHHs)#GgGZ_7|vkt9izl_&PBrsu@}L`X{95-2jf99K)0=*N)VxBX2q z((vkpP2RneSIiIUEnGb?VqbMb=Zia+rF~+iqslydE34cSLJ&BJW^3knX@M;t*b=EA zNvGzv41Ld_T+WT#XjDB840vovUU^FtN_)G}7v)1lPetgpEK9YS^OWFkPoE{ovj^=@ zO9N$S=G$1ecndT_=5ehth2Lmd1II-PuT~C9`XVePw$y8J#dpZ?Tss<6wtVglm(Ok7 z3?^oi@pPio6l&!z8JY(pJvG=*pI?GIOu}e^EB6QYk$#FJQ%^AIK$I4epJ+9t?KjqA+bkj&PQ*|vLttme+`9G=L% ziadyMw_7-M)hS(3E$QGNCu|o23|%O+VN7;Qggp?PB3K-iSeBa2b}V4_wY`G1Jsfz4 z9|SdB^;|I8E8gWqHKx!vj_@SMY^hLEIbSMCuE?WKq=c2mJK z8LoG-pnY!uhqFv&L?yEuxo{dpMTsmCn)95xanqBrNPTgXP((H$9N${Ow~Is-FBg%h z53;|Y5$MUN)9W2HBe2TD`ct^LHI<(xWrw}$qSoei?}s)&w$;&!14w6B6>Yr6Y8b)S z0r71`WmAvJJ`1h&poLftLUS6Ir zC$bG9!Im_4Zjse)#K=oJM9mHW1{%l8sz$1o?ltdKlLTxWWPB>Vk22czVt|1%^wnN@*!l)}?EgtvhC>vlHm^t+ogpgHI1_$1ox9e;>0!+b(tBrmXRB`PY1vp-R**8N7 zGP|QqI$m(Rdu#=(?!(N}G9QhQ%o!aXE=aN{&wtGP8|_qh+7a_j_sU5|J^)vxq;# zjvzLn%_QPHZZIWu1&mRAj;Sa_97p_lLq_{~j!M9N^1yp3U_SxRqK&JnR%6VI#^E12 z>CdOVI^_9aPK2eZ4h&^{pQs}xsijXgFYRIxJ~N7&BB9jUR1fm!(xl)mvy|3e6-B3j zJn#ajL;bFTYJ2+Q)tDjx=3IklO@Q+FFM}6UJr6km7hj7th9n_&JR7fnqC!hTZoM~T zBeaVFp%)0cbPhejX<8pf5HyRUj2>aXnXBqDJe73~J%P(2C?-RT{c3NjE`)om! zl$uewSgWkE66$Kb34+QZZvRn`fob~Cl9=cRk@Es}KQm=?E~CE%spXaMO6YmrMl%9Q zlA3Q$3|L1QJ4?->UjT&CBd!~ru{Ih^in&JXO=|<6J!&qp zRe*OZ*cj5bHYlz!!~iEKcuE|;U4vN1rk$xq6>bUWD*u(V@8sG^7>kVuo(QL@Ki;yL zWC!FT(q{E8#on>%1iAS0HMZDJg{Z{^!De(vSIq&;1$+b)oRMwA3nc3mdTSG#3uYO_ z>+x;7p4I;uHz?ZB>dA-BKl+t-3IB!jBRgdvAbW!aJ(Q{aT>+iz?91`C-xbe)IBoND z9_Xth{6?(y3rddwY$GD65IT#f3<(0o#`di{sh2gm{dw*#-Vnc3r=4==&PU^hCv$qd zjw;>i&?L*Wq#TxG$mFIUf>eK+170KG;~+o&1;Tom9}}mKo23KwdEM6UonXgc z!6N(@k8q@HPw{O8O!lAyi{rZv|DpgfU{py+j(X_cwpKqcalcqKIr0kM^%Br3SdeD> zHSKV94Yxw;pjzDHo!Q?8^0bb%L|wC;4U^9I#pd5O&eexX+Im{ z?jKnCcsE|H?{uGMqVie_C~w7GX)kYGWAg%-?8|N_1#W-|4F)3YTDC+QSq1s!DnOML3@d`mG%o2YbYd#jww|jD$gotpa)kntakp#K;+yo-_ZF9qrNZw<%#C zuPE@#3RocLgPyiBZ+R_-FJ_$xP!RzWm|aN)S+{$LY9vvN+IW~Kf3TsEIvP+B9Mtm! zpfNNxObWQpLoaO&cJh5>%slZnHl_Q~(-Tfh!DMz(dTWld@LG1VRF`9`DYKhyNv z2pU|UZ$#_yUx_B_|MxUq^glT}O5Xt(Vm4Mr02><%C)@v;vPb@pT$*yzJ4aPc_FZ3z z3}PLoMBIM>q_9U2rl^sGhk1VUJ89=*?7|v`{!Z{6bqFMq(mYiA?%KbsI~JwuqVA9$H5vDE+VocjX+G^%bieqx->s;XWlKcuv(s%y%D5Xbc9+ zc(_2nYS1&^yL*ey664&4`IoOeDIig}y-E~_GS?m;D!xv5-xwz+G`5l6V+}CpeJDi^ z%4ed$qowm88=iYG+(`ld5Uh&>Dgs4uPHSJ^TngXP_V6fPyl~>2bhi20QB%lSd#yYn zO05?KT1z@?^-bqO8Cg`;ft>ilejsw@2%RR7;`$Vs;FmO(Yr3Fp`pHGr@P2hC%QcA|X&N2Dn zYf`MqXdHi%cGR@%y7Rg7?d3?an){s$zA{!H;Ie5exE#c~@NhQUFG8V=SQh%UxUeiV zd7#UcYqD=lk-}sEwlpu&H^T_V0{#G?lZMxL7ih_&{(g)MWBnCZxtXg znr#}>U^6!jA%e}@Gj49LWG@*&t0V>Cxc3?oO7LSG%~)Y5}f7vqUUnQ;STjdDU}P9IF9d9<$;=QaXc zL1^X7>fa^jHBu_}9}J~#-oz3Oq^JmGR#?GO7b9a(=R@fw@}Q{{@`Wy1vIQ#Bw?>@X z-_RGG@wt|%u`XUc%W{J z>iSeiz8C3H7@St3mOr_mU+&bL#Uif;+Xw-aZdNYUpdf>Rvu0i0t6k*}vwU`XNO2he z%miH|1tQ8~ZK!zmL&wa3E;l?!!XzgV#%PMVU!0xrDsNNZUWKlbiOjzH-1Uoxm8E#r`#2Sz;-o&qcqB zC-O_R{QGuynW14@)7&@yw1U}uP(1cov)twxeLus0s|7ayrtT8c#`&2~Fiu2=R;1_4bCaD=*E@cYI>7YSnt)nQc zohw5CsK%m?8Ack)qNx`W0_v$5S}nO|(V|RZKBD+btO?JXe|~^Qqur%@eO~<8-L^9d z=GA3-V14ng9L29~XJ>a5k~xT2152zLhM*@zlp2P5Eu}bywkcqR;ISbas&#T#;HZSf z2m69qTV(V@EkY(1Dk3`}j)JMo%ZVJ*5eB zYOjIisi+igK0#yW*gBGj?@I{~mUOvRFQR^pJbEbzFxTubnrw(Muk%}jI+vXmJ;{Q6 zrSobKD>T%}jV4Ub?L1+MGOD~0Ir%-`iTnWZN^~YPrcP5y3VMAzQ+&en^VzKEb$K!Q z<7Dbg&DNXuow*eD5yMr+#08nF!;%4vGrJI++5HdCFcGLfMW!KS*Oi@=7hFwDG!h2< zPunUEAF+HncQkbfFj&pbzp|MU*~60Z(|Ik%Tn{BXMN!hZOosNIseT?R;A`W?=d?5X zK(FB=9mZusYahp|K-wyb={rOpdn=@;4YI2W0EcbMKyo~-#^?h`BA9~o285%oY zfifCh5Lk$SY@|2A@a!T2V+{^!psQkx4?x0HSV`(w9{l75QxMk!)U52Lbhn{8ol?S) zCKo*7R(z!uk<6*qO=wh!Pul{(qq6g6xW;X68GI_CXp`XwO zxuSgPRAtM8K7}5E#-GM!*ydOOG_{A{)hkCII<|2=ma*71ci_-}VPARm3crFQjLYV! z9zbz82$|l01mv`$WahE2$=fAGWkd^X2kY(J7iz}WGS z@%MyBEO=A?HB9=^?nX`@nh;7;laAjs+fbo!|K^mE!tOB>$2a_O0y-*uaIn8k^6Y zSbuv;5~##*4Y~+y7Z5O*3w4qgI5V^17u*ZeupVGH^nM&$qmAk|anf*>r zWc5CV;-JY-Z@Uq1Irpb^O`L_7AGiqd*YpGUShb==os$uN3yYvb`wm6d=?T*it&pDk zo`vhw)RZX|91^^Wa_ti2zBFyWy4cJu#g)_S6~jT}CC{DJ_kKpT`$oAL%b^!2M;JgT zM3ZNbUB?}kP(*YYvXDIH8^7LUxz5oE%kMhF!rnPqv!GiY0o}NR$OD=ITDo9r%4E>E0Y^R(rS^~XjWyVI6 zMOR5rPXhTp*G*M&X#NTL`Hu*R+u*QNoiOKg4CtNPrjgH>c?Hi4MUG#I917fx**+pJfOo!zFM&*da&G_x)L(`k&TPI*t3e^{crd zX<4I$5nBQ8Ax_lmNRa~E*zS-R0sxkz`|>7q_?*e%7bxqNm3_eRG#1ae3gtV9!fQpY z+!^a38o4ZGy9!J5sylDxZTx$JmG!wg7;>&5H1)>f4dXj;B+@6tMlL=)cLl={jLMxY zbbf1ax3S4>bwB9-$;SN2?+GULu;UA-35;VY*^9Blx)Jwyb$=U!D>HhB&=jSsd^6yw zL)?a|>GxU!W}ocTC(?-%z3!IUhw^uzc`Vz_g>-tv)(XA#JK^)ZnC|l1`@CdX1@|!| z_9gQ)7uOf?cR@KDp97*>6X|;t@Y`k_N@)aH7gY27)COv^P3ya9I{4z~vUjLR9~z1Z z5=G{mVtKH*&$*t0@}-i_v|3B$AHHYale7>E+jP`ClqG%L{u;*ff_h@)al?RuL7tOO z->;I}>%WI{;vbLP3VIQ^iA$4wl6@0sDj|~112Y4OFjMs`13!$JGkp%b&E8QzJw_L5 zOnw9joc0^;O%OpF$Qp)W1HI!$4BaXX84`%@#^dk^hFp^pQ@rx4g(8Xjy#!X%+X5Jd@fs3amGT`}mhq#L97R>OwT5-m|h#yT_-v@(k$q7P*9X~T*3)LTdzP!*B} z+SldbVWrrwQo9wX*%FyK+sRXTa@O?WM^FGWOE?S`R(0P{<6p#f?0NJvnBia?k^fX2 zNQs7K-?EijgHJY}&zsr;qJ<*PCZUd*x|dD=IQPUK_nn)@X4KWtqoJNHkT?ZWL_hF? zS8lp2(q>;RXR|F;1O}EE#}gCrY~#n^O`_I&?&z5~7N;zL0)3Tup`%)oHMK-^r$NT% zbFg|o?b9w(q@)6w5V%si<$!U<#}s#x@0aX-hP>zwS#9*75VXA4K*%gUc>+yzupTDBOKH8WR4V0pM(HrfbQ&eJ79>HdCvE=F z|J>s;;iDLB^3(9}?biKbxf1$lI!*Z%*0&8UUq}wMyPs_hclyQQi4;NUY+x2qy|0J; zhn8;5)4ED1oHwg+VZF|80<4MrL97tGGXc5Sw$wAI#|2*cvQ=jB5+{AjMiDHmhUC*a zlmiZ`LAuAn_}hftXh;`Kq0zblDk8?O-`tnilIh|;3lZp@F_osJUV9`*R29M?7H{Fy z`nfVEIDIWXmU&YW;NjU8)EJpXhxe5t+scf|VXM!^bBlwNh)~7|3?fWwo_~ZFk(22% zTMesYw+LNx3J-_|DM~`v93yXe=jPD{q;li;5PD?Dyk+b? zo21|XpT@)$BM$%F=P9J19Vi&1#{jM3!^Y&fr&_`toi`XB1!n>sbL%U9I5<7!@?t)~ z;&H%z>bAaQ4f$wIzkjH70;<8tpUoxzKrPhn#IQfS%9l5=Iu))^XC<58D!-O z{B+o5R^Z21H0T9JQ5gNJnqh#qH^na|z92=hONIM~@_iuOi|F>jBh-?aA20}Qx~EpDGElELNn~|7WRXRFnw+Wdo`|# zBpU=Cz3z%cUJ0mx_1($X<40XEIYz(`noWeO+x#yb_pwj6)R(__%@_Cf>txOQ74wSJ z0#F3(zWWaR-jMEY$7C*3HJrohc79>MCUu26mfYN)f4M~4gD`}EX4e}A!U}QV8!S47 z6y-U-%+h`1n`*pQuKE%Av0@)+wBZr9mH}@vH@i{v(m-6QK7Ncf17x_D=)32`FOjjo zg|^VPf5c6-!FxN{25dvVh#fog=NNpXz zfB$o+0jbRkHH{!TKhE709f+jI^$3#v1Nmf80w`@7-5$1Iv_`)W^px8P-({xwb;D0y z7LKDAHgX<84?l!I*Dvi2#D@oAE^J|g$3!)x1Ua;_;<@#l1fD}lqU2_tS^6Ht$1Wl} zBESo7o^)9-Tjuz$8YQSGhfs{BQV6zW7dA?0b(Dbt=UnQs&4zHfe_sj{RJ4uS-vQpC zX;Bbsuju4%!o8?&m4UZU@~ZZjeFF6ex2ss5_60_JS_|iNc+R0GIjH1@Z z=rLT9%B|WWgOrR7IiIwr2=T;Ne?30M!@{%Qf8o`!>=s<2CBpCK_TWc(DX51>e^xh8 z&@$^b6CgOd7KXQV&Y4%}_#uN*mbanXq(2=Nj`L7H7*k(6F8s6{FOw@(DzU`4-*77{ zF+dxpv}%mFpYK?>N_2*#Y?oB*qEKB}VoQ@bzm>ptmVS_EC(#}Lxxx730trt0G)#$b zE=wVvtqOct1%*9}U{q<)2?{+0TzZzP0jgf9*)arV)*e!f`|jgT{7_9iS@e)recI#z zbzolURQ+TOzE!ymqvBY7+5NnAbWxvMLsLTwEbFqW=CPyCsmJ}P1^V30|D5E|p3BC5 z)3|qgw@ra7aXb-wsa|l^in~1_fm{7bS9jhVRkYVO#U{qMp z)Wce+|DJ}4<2gp8r0_xfZpMo#{Hl2MfjLcZdRB9(B(A(f;+4s*FxV{1F|4d`*sRNd zp4#@sEY|?^FIJ;tmH{@keZ$P(sLh5IdOk@k^0uB^BWr@pk6mHy$qf&~rI>P*a;h0C{%oA*i!VjWn&D~O#MxN&f@1Po# zKN+ zrGrkSjcr?^R#nGl<#Q722^wbYcgW@{+6CBS<1@%dPA8HC!~a`jTz<`g_l5N1M@9wn9GOAZ>nqNgq!yOCbZ@1z`U_N`Z>}+1HIZxk*5RDc&rd5{3qjRh8QmT$VyS;jK z;AF+r6XnnCp=wQYoG|rT2@8&IvKq*IB_WvS%nt%e{MCFm`&W*#LXc|HrD?nVBo=(8*=Aq?u$sDA_sC_RPDUiQ+wnIJET8vx$&fxkW~kP9qXKt zozR)@xGC!P)CTkjeWvXW5&@2?)qt)jiYWWBU?AUtzAN}{JE1I)dfz~7$;}~BmQF`k zpn11qmObXwRB8&rnEG*#4Xax3XBkKlw(;tb?Np^i+H8m(Wyz9k{~ogba@laiEk;2! zV*QV^6g6(QG%vX5Um#^sT&_e`B1pBW5yVth~xUs#0}nv?~C#l?W+9Lsb_5)!71rirGvY zTIJ$OPOY516Y|_014sNv+Z8cc5t_V=i>lWV=vNu#!58y9Zl&GsMEW#pPYPYGHQ|;vFvd*9eM==$_=vc7xnyz0~ zY}r??$<`wAO?JQk@?RGvkWVJlq2dk9vB(yV^vm{=NVI8dhsX<)O(#nr9YD?I?(VmQ z^r7VfUBn<~p3()8yOBjm$#KWx!5hRW)5Jl7wY@ky9lNM^jaT##8QGVsYeaVywmpv>X|Xj7gWE1Ezai&wVLt3p)k4w~yrskT-!PR!kiyQlaxl(( zXhF%Q9x}1TMt3~u@|#wWm-Vq?ZerK={8@~&@9r5JW}r#45#rWii};t`{5#&3$W)|@ zbAf2yDNe0q}NEUvq_Quq3cTjcw z@H_;$hu&xllCI9CFDLuScEMg|x{S7GdV8<&Mq=ezDnRZAyX-8gv97YTm0bg=d)(>N z+B2FcqvI9>jGtnK%eO%y zoBPkJTk%y`8TLf4)IXPBn`U|9>O~WL2C~C$z~9|0m*YH<-vg2CD^SX#&)B4ngOSG$ zV^wmy_iQk>dfN@Pv(ckfy&#ak@MLC7&Q6Ro#!ezM*VEh`+b3Jt%m(^T&p&WJ2Oqvj zs-4nq0TW6cv~(YI$n0UkfwN}kg3_fp?(ijSV#tR9L0}l2qjc7W?i*q01=St0eZ=4h zyGQbEw`9OEH>NMuIe)hVwYHsGERWOD;JxEiO7cQv%pFCeR+IyhwQ|y@&^24k+|8fD zLiOWFNJ2&vu2&`Jv96_z-Cd5RLgmeY3*4rDOQo?Jm`;I_(+ejsPM03!ly!*Cu}Cco zrQSrEDHNyzT(D5s1rZq!8#?f6@v6dB7a-aWs(Qk>N?UGAo{gytlh$%_IhyL7h?DLXDGx zgxGEBQoCAWo-$LRvM=F5MTle`M})t3vVv;2j0HZY&G z22^iGhV@uaJh(XyyY%} zd4iH_UfdV#T=3n}(Lj^|n;O4|$;xhu*8T3hR1mc_A}fK}jfZ7LX~*n5+`8N2q#rI$ z@<_2VANlYF$vIH$ zl<)+*tIWW78IIINA7Rr7i{<;#^yzxoLNkXL)eSs=%|P>$YQIh+ea_3k z_s7r4%j7%&*NHSl?R4k%1>Z=M9o#zxY!n8sL5>BO-ZP;T3Gut>iLS@U%IBrX6BA3k z)&@q}V8a{X<5B}K5s(c(LQ=%v1ocr`t$EqqY0EqVjr65usa=0bkf|O#ky{j3)WBR(((L^wmyHRzoWuL2~WTC=`yZ zn%VX`L=|Ok0v7?s>IHg?yArBcync5rG#^+u)>a%qjES%dRZoIyA8gQ;StH z1Ao7{<&}6U=5}4v<)1T7t!J_CL%U}CKNs-0xWoTTeqj{5{?Be$L0_tk>M9o8 zo371}S#30rKZFM{`H_(L`EM9DGp+Mifk&IP|C2Zu_)Ghr4Qtpmkm1osCf@%Z$%t+7 zYH$Cr)Ro@3-QDeQJ8m+x6%;?YYT;k6Z0E-?kr>x33`H%*ueBD7Zx~3&HtWn0?2Wt} zTG}*|v?{$ajzt}xPzV%lL1t-URi8*Zn)YljXNGDb>;!905Td|mpa@mHjIH%VIiGx- zd@MqhpYFu4_?y5N4xiHn3vX&|e6r~Xt> zZG`aGq|yTNjv;9E+Txuoa@A(9V7g?1_T5FzRI;!=NP1Kqou1z5?%X~Wwb{trRfd>i z8&y^H)8YnKyA_Fyx>}RNmQIczT?w2J4SNvI{5J&}Wto|8FR(W;Qw#b1G<1%#tmYzQ zQ2mZA-PAdi%RQOhkHy9Ea#TPSw?WxwL@H@cbkZwIq0B!@ns}niALidmn&W?!Vd4Gj zO7FiuV4*6Mr^2xlFSvM;Cp_#r8UaqIzHJQg_z^rEJw&OMm_8NGAY2)rKvki|o1bH~ z$2IbfVeY2L(^*rMRU1lM5Y_sgrDS`Z??nR2lX;zyR=c%UyGb*%TC-Dil?SihkjrQy~TMv6;BMs7P8il`H7DmpVm@rJ;b)hW)BL)GjS154b*xq-NXq2cwE z^;VP7ua2pxvCmxrnqUYQMH%a%nHmwmI33nJM(>4LznvY*k&C0{8f*%?zggpDgkuz&JBx{9mfb@wegEl2v!=}Sq2Gaty0<)UrOT0{MZtZ~j5y&w zXlYa_jY)I_+VA-^#mEox#+G>UgvM!Ac8zI<%JRXM_73Q!#i3O|)lOP*qBeJG#BST0 zqohi)O!|$|2SeJQo(w6w7%*92S})XfnhrH_Z8qe!G5>CglP=nI7JAOW?(Z29;pXJ9 zR9`KzQ=WEhy*)WH>$;7Cdz|>*i>=##0bB)oU0OR>>N<21e4rMCHDemNi2LD>Nc$;& zQRFthpWniC1J6@Zh~iJCoLOxN`oCKD5Q4r%ynwgUKPlIEd#?QViIqovY|czyK8>6B zSP%{2-<;%;1`#0mG^B(8KbtXF;Nf>K#Di72UWE4gQ%(_26Koiad)q$xRL~?pN71ZZ zujaaCx~jXjygw;rI!WB=xrOJO6HJ!!w}7eiivtCg5K|F6$EXa)=xUC za^JXSX98W`7g-tm@uo|BKj39Dl;sg5ta;4qjo^pCh~{-HdLl6qI9Ix6f$+qiZ$}s= zNguKrU;u+T@ko(Vr1>)Q%h$?UKXCY>3se%&;h2osl2D zE4A9bd7_|^njDd)6cI*FupHpE3){4NQ*$k*cOWZ_?CZ>Z4_fl@n(mMnYK62Q1d@+I zr&O))G4hMihgBqRIAJkLdk(p(D~X{-oBUA+If@B}j& zsHbeJ3RzTq96lB7d($h$xTeZ^gP0c{t!Y0c)aQE;$FY2!mACg!GDEMKXFOPI^)nHZ z`aSPJpvV0|bbrzhWWkuPURlDeN%VT8tndV8?d)eN*i4I@u zVKl^6{?}A?P)Fsy?3oi#clf}L18t;TjNI2>eI&(ezDK7RyqFxcv%>?oxUlonv(px) z$vnPzRH`y5A(x!yOIfL0bmgeMQB$H5wenx~!ujQK*nUBW;@Em&6Xv2%s(~H5WcU2R z;%Nw<$tI)a`Ve!>x+qegJnQsN2N7HaKzrFqM>`6R*gvh%O*-%THt zrB$Nk;lE;z{s{r^PPm5qz(&lM{sO*g+W{sK+m3M_z=4=&CC>T`{X}1Vg2PEfSj2x_ zmT*(x;ov%3F?qoEeeM>dUn$a*?SIGyO8m806J1W1o+4HRhc2`9$s6hM#qAm zChQ87b~GEw{ADfs+5}FJ8+|bIlIv(jT$Ap#hSHoXdd9#w<#cA<1Rkq^*EEkknUd4& zoIWIY)sAswy6fSERVm&!SO~#iN$OgOX*{9@_BWFyJTvC%S++ilSfCrO(?u=Dc?CXZ zzCG&0yVR{Z`|ZF0eEApWEo#s9osV>F{uK{QA@BES#&;#KsScf>y zvs?vIbI>VrT<*!;XmQS=bhq%46-aambZ(8KU-wOO2=en~D}MCToB_u;Yz{)1ySrPZ z@=$}EvjTdzTWU7c0ZI6L8=yP+YRD_eMMos}b5vY^S*~VZysrkq<`cK3>>v%uy7jgq z0ilW9KjVDHLv0b<1K_`1IkbTOINs0=m-22c%M~l=^S}%hbli-3?BnNq?b`hx^HX2J zIe6ECljRL0uBWb`%{EA=%!i^4sMcj+U_TaTZRb+~GOk z^ZW!nky0n*Wb*r+Q|9H@ml@Z5gU&W`(z4-j!OzC1wOke`TRAYGZVl$PmQ16{3196( zO*?`--I}Qf(2HIwb2&1FB^!faPA2=sLg(@6P4mN)>Dc3i(B0;@O-y2;lM4akD>@^v z=u>*|!s&9zem70g7zfw9FXl1bpJW(C#5w#uy5!V?Q(U35A~$dR%LDVnq@}kQm13{} zd53q3N(s$Eu{R}k2esbftfjfOITCL;jWa$}(mmm}d(&7JZ6d3%IABCapFFYjdEjdK z&4Edqf$G^MNAtL=uCDRs&Fu@FXRgX{*0<(@c3|PNHa>L%zvxWS={L8%qw`STm+=Rd zA}FLspESSIpE_^41~#5yI2bJ=9`oc;GIL!JuW&7YetZ?0H}$$%8rW@*J37L-~Rsx!)8($nI4 zZhcZ2^=Y+p4YPl%j!nFJA|*M^gc(0o$i3nlphe+~-_m}jVkRN{spFs(o0ajW@f3K{ zDV!#BwL322CET$}Y}^0ixYj2w>&Xh12|R8&yEw|wLDvF!lZ#dOTHM9pK6@Nm-@9Lnng4ZHBgBSrr7KI8YCC9DX5Kg|`HsiwJHg2(7#nS;A{b3tVO?Z% za{m5b3rFV6EpX;=;n#wltDv1LE*|g5pQ+OY&*6qCJZc5oDS6Z6JD#6F)bWxZSF@q% z+1WV;m!lRB!n^PC>RgQCI#D1br_o^#iPk>;K2hB~0^<~)?p}LG%kigm@moD#q3PE+ zA^Qca)(xnqw6x>XFhV6ku9r$E>bWNrVH9fum0?4s?Rn2LG{Vm_+QJHse6xa%nzQ?k zKug4PW~#Gtb;#5+9!QBgyB@q=sk9=$S{4T>wjFICStOM?__fr+Kei1 z3j~xPqW;W@YkiUM;HngG!;>@AITg}vAE`M2Pj9Irl4w1fo4w<|Bu!%rh%a(Ai^Zhi zs92>v5;@Y(Zi#RI*ua*h`d_7;byQSa*v9E{2x$<-_=5Z<7{%)}4XExANcz@rK69T0x3%H<@frW>RA8^swA+^a(FxK| zFl3LD*ImHN=XDUkrRhp6RY5$rQ{bRgSO*(vEHYV)3Mo6Jy3puiLmU&g82p{qr0F?ohmbz)f2r{X2|T2 z$4fdQ=>0BeKbiVM!e-lIIs8wVTuC_m7}y4A_%ikI;Wm5$9j(^Y z(cD%U%k)X>_>9~t8;pGzL6L-fmQO@K; zo&vQzMlgY95;1BSkngY)e{`n0!NfVgf}2mB3t}D9@*N;FQ{HZ3Pb%BK6;5#-O|WI( zb6h@qTLU~AbVW#_6?c!?Dj65Now7*pU{h!1+eCV^KCuPAGs28~3k@ueL5+u|Z-7}t z9|lskE`4B7W8wMs@xJa{#bsCGDFoRSNSnmNYB&U7 zVGKWe%+kFB6kb)e;TyHfqtU6~fRg)f|>=5(N36)0+C z`hv65J<$B}WUc!wFAb^QtY31yNleq4dzmG`1wHTj=c*=hay9iD071Hc?oYoUk|M*_ zU1GihAMBsM@5rUJ(qS?9ZYJ6@{bNqJ`2Mr+5#hKf?doa?F|+^IR!8lq9)wS3tF_9n zW_?hm)G(M+MYb?V9YoX^_mu5h-LP^TL^!Q9Z7|@sO(rg_4+@=PdI)WL(B7`!K^ND- z-uIuVDCVEdH_C@c71YGYT^_Scf_dhB8Z2Xy6vGtBSlYud9vggOqv^L~F{BraSE_t} zIkP+Hp2&nH^-MNEs}^`oMLy11`PQW$T|K(`Bu*(f@)mv1-qY(_YG&J2M2<7k;;RK~ zL{Fqj9yCz8(S{}@c)S!65aF<=&eLI{hAMErCx&>i7OeDN>okvegO87OaG{Jmi<|}D zaT@b|0X{d@OIJ7zvT>r+eTzgLq~|Dpu)Z&db-P4z*`M$UL51lf>FLlq6rfG)%doyp z)3kk_YIM!03eQ8Vu_2fg{+osaEJPtJ-s36R+5_AEG12`NG)IQ#TF9c@$99%0iye+ zUzZ57=m2)$D(5Nx!n)=5Au&O0BBgwxIBaeI(mro$#&UGCr<;C{UjJVAbVi%|+WP(a zL$U@TYCxJ=1{Z~}rnW;7UVb7+ZnzgmrogDxhjLGo>c~MiJAWs&&;AGg@%U?Y^0JhL ze(x6Z74JG6FlOFK(T}SXQfhr}RIFl@QXKnIcXYF)5|V~e-}suHILKT-k|<*~Ij|VF zC;t@=uj=hot~*!C68G8hTA%8SzOfETOXQ|3FSaIEjvBJp(A)7SWUi5!Eu#yWgY+;n zlm<$+UDou*V+246_o#V4kMdto8hF%%Lki#zPh}KYXmMf?hrN0;>Mv%`@{0Qn`Ujp) z=lZe+13>^Q!9zT);H<(#bIeRWz%#*}sgUX9P|9($kexOyKIOc`dLux}c$7It4u|Rl z6SSkY*V~g_B-hMPo_ak>>z@AVQ(_N)VY2kB3IZ0G(iDUYw+2d7W^~(Jq}KY=JnWS( z#rzEa&0uNhJ>QE8iiyz;n2H|SV#Og+wEZv=f2%1ELX!SX-(d3tEj$5$1}70Mp<&eI zCkfbByL7af=qQE@5vDVxx1}FSGt_a1DoE3SDI+G)mBAna)KBG4p8Epxl9QZ4BfdAN zFnF|Y(umr;gRgG6NLQ$?ZWgllEeeq~z^ZS7L?<(~O&$5|y)Al^iMKy}&W+eMm1W z7EMU)u^ke(A1#XCV>CZ71}P}0x)4wtHO8#JRG3MA-6g=`ZM!FcICCZ{IEw8Dm2&LQ z1|r)BUG^0GzI6f946RrBlfB1Vs)~8toZf~7)+G;pv&XiUO(%5bm)pl=p>nV^o*;&T z;}@oZSibzto$arQgfkp|z4Z($P>dTXE{4O=vY0!)kDO* zGF8a4wq#VaFpLfK!iELy@?-SeRrdz%F*}hjKcA*y@mj~VD3!it9lhRhX}5YOaR9$} z3mS%$2Be7{l(+MVx3 z(4?h;P!jnRmX9J9sYN#7i=iyj_5q7n#X(!cdqI2lnr8T$IfOW<_v`eB!d9xY1P=2q&WtOXY=D9QYteP)De?S4}FK6#6Ma z=E*V+#s8>L;8aVroK^6iKo=MH{4yEZ_>N-N z`(|;aOATba1^asjxlILk<4}f~`39dBFlxj>Dw(hMYKPO3EEt1@S`1lxFNM+J@uB7T zZ8WKjz7HF1-5&2=l=fqF-*@>n5J}jIxdDwpT?oKM3s8Nr`x8JnN-kCE?~aM1H!hAE z%%w(3kHfGwMnMmNj(SU(w42OrC-euI>Dsjk&jz3ts}WHqmMpzQ3vZrsXrZ|}+MHA7 z068obeXZTsO*6RS@o3x80E4ok``rV^Y3hr&C1;|ZZ0|*EKO`$lECUYG2gVFtUTw)R z4Um<0ZzlON`zTdvVdL#KFoMFQX*a5wM0Czp%wTtfK4Sjs)P**RW&?lP$(<}q%r68Z zS53Y!d@&~ne9O)A^tNrXHhXBkj~$8j%pT1%%mypa9AW5E&s9)rjF4@O3ytH{0z6riz|@< zB~UPh*wRFg2^7EbQrHf0y?E~dHlkOxof_a?M{LqQ^C!i2dawHTPYUE=X@2(3<=OOxs8qn_(y>pU>u^}3y&df{JarR0@VJn0f+U%UiF=$Wyq zQvnVHESil@d|8&R<%}uidGh7@u^(%?$#|&J$pvFC-n8&A>utA=n3#)yMkz+qnG3wd zP7xCnF|$9Dif@N~L)Vde3hW8W!UY0BgT2v(wzp;tlLmyk2%N|0jfG$%<;A&IVrOI< z!L)o>j>;dFaqA3pL}b-Je(bB@VJ4%!JeX@3x!i{yIeIso^=n?fDX`3bU=eG7sTc%g%ye8$v8P@yKE^XD=NYxTb zbf!Mk=h|otpqjFaA-vs5YOF-*GwWPc7VbaOW&stlANnCN8iftFMMrUdYNJ_Bnn5Vt zxfz@Ah|+4&P;reZxp;MmEI7C|FOv8NKUm8njF7Wb6Gi7DeODLl&G~}G4be&*Hi0Qw z5}77vL0P+7-B%UL@3n1&JPxW^d@vVwp?u#gVcJqY9#@-3X{ok#UfW3<1fb%FT`|)V~ggq z(3AUoUS-;7)^hCjdT0Kf{i}h)mBg4qhtHHBti=~h^n^OTH5U*XMgDLIR@sre`AaB$ zg)IGBET_4??m@cx&c~bA80O7B8CHR7(LX7%HThkeC*@vi{-pL%e)yXp!B2InafbDF zjPXf1mko3h59{lT6EEbxKO1Z5GF71)WwowO6kY|6tjSVSWdQ}NsK2x{>i|MKZK8%Q zfu&_0D;CO-Jg0#YmyfctyJ!mRJp)e#@O0mYdp|8x;G1%OZQ3Q847YWTyy|%^cpA;m zze0(5p{tMu^lDkpe?HynyO?a1$_LJl2L&mpeKu%8YvgRNr=%2z${%WThHG=vrWY@4 zsA`OP#O&)TetZ>s%h!=+CE15lOOls&nvC~$Qz0Ph7tHiP;O$i|eDwpT{cp>+)0-|; zY$|bB+Gbel>5aRN3>c0x)4U=|X+z+{ zn*_p*EQoquRL+=+p;=lm`d71&1NqBz&_ph)MXu(Nv6&XE7(RsS)^MGj5Q?Fwude-(sq zjJ>aOq!7!EN>@(fK7EE#;i_BGvli`5U;r!YA{JRodLBc6-`n8K+Fjgwb%sX;j=qHQ z7&Tr!)!{HXoO<2BQrV9Sw?JRaLXV8HrsNevvnf>Y-6|{T!pYLl7jp$-nEE z#X!4G4L#K0qG_4Z;Cj6=;b|Be$hi4JvMH!-voxqx^@8cXp`B??eFBz2lLD8RRaRGh zn7kUfy!YV~p(R|p7iC1Rdgt$_24i0cd-S8HpG|`@my70g^y`gu%#Tf_L21-k?sRRZHK&at(*ED0P8iw{7?R$9~OF$Ko;Iu5)ur5<->x!m93Eb zFYpIx60s=Wxxw=`$aS-O&dCO_9?b1yKiPCQmSQb>T)963`*U+Ydj5kI(B(B?HNP8r z*bfSBpSu)w(Z3j7HQoRjUG(+d=IaE~tv}y14zHHs|0UcN52fT8V_<@2ep_ee{QgZG zmgp8iv4V{k;~8@I%M3<#B;2R>Ef(Gg_cQM7%}0s*^)SK6!Ym+~P^58*wnwV1BW@eG z4sZLqsUvBbFsr#8u7S1r4teQ;t)Y@jnn_m5jS$CsW1um!p&PqAcc8!zyiXHVta9QC zY~wCwCF0U%xiQPD_INKtTb;A|Zf29(mu9NI;E zc-e>*1%(LSXB`g}kd`#}O;veb<(sk~RWL|f3ljxCnEZDdNSTDV6#Td({6l&y4IjKF z^}lIUq*ZUqgTPumD)RrCN{M^jhY>E~1pn|KOZ5((%F)G|*ZQ|r4zIbrEiV%42hJV8 z3xS)=!X1+=olbdGJ=yZil?oXLct8FM{(6ikLL3E%=q#O6(H$p~gQu6T8N!plf!96| z&Q3=`L~>U0zZh;z(pGR2^S^{#PrPxTRHD1RQOON&f)Siaf`GLj#UOk&(|@0?zm;Sx ztsGt8=29-MZs5CSf1l1jNFtNt5rFNZxJPvkNu~2}7*9468TWm>nN9TP&^!;J{-h)_ z7WsHH9|F%I`Pb!>KAS3jQWKfGivTVkMJLO-HUGM_a4UQ_%RgL6WZvrW+Z4ujZn;y@ zz9$=oO!7qVTaQAA^BhX&ZxS*|5dj803M=k&2%QrXda`-Q#IoZL6E(g+tN!6CA!CP* zCpWtCujIea)ENl0liwVfj)Nc<9mV%+e@=d`haoZ*`B7+PNjEbXBkv=B+Pi^~L#EO$D$ZqTiD8f<5$eyb54-(=3 zh)6i8i|jp(@OnRrY5B8t|LFXFQVQ895n*P16cEKTrT*~yLH6Z4e*bZ5otpRDri&+A zfNbK1D5@O=sm`fN=WzWyse!za5n%^+6dHPGX#8DyIK>?9qyX}2XvBWVqbP%%D)7$= z=#$WulZlZR<{m#gU7lwqK4WS1Ne$#_P{b17qe$~UOXCl>5b|6WVh;5vVnR<%d+Lnp z$uEmML38}U4vaW8>shm6CzB(Wei3s#NAWE3)a2)z@i{4jTn;;aQS)O@l{rUM`J@K& l00vQ5JBs~;vo!vr%%-k{2_Fq1Mn4QF81S)AQ99zk{{c4yR+0b! literal 0 HcmV?d00001 diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 00000000..7015f6be --- /dev/null +++ b/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,7 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip +networkTimeout=10000 +validateDistributionUrl=true +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/gradlew b/gradlew new file mode 100755 index 00000000..1aa94a42 --- /dev/null +++ b/gradlew @@ -0,0 +1,249 @@ +#!/bin/sh + +# +# Copyright © 2015-2021 the original 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 POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# +############################################################################## + +# Attempt to set APP_HOME + +# Resolve links: $0 may be a link +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac +done + +# This is normally unused +# shellcheck disable=SC2034 +APP_BASE_NAME=${0##*/} +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD=maximum + +warn () { + echo "$*" +} >&2 + +die () { + echo + echo "$*" + echo + exit 1 +} >&2 + +# 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 ;; #( + MSYS* | 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 + if ! command -v java >/dev/null 2>&1 + then + 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 +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac +fi + +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. + +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done +fi + + +# 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"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' + +exec "$JAVACMD" "$@" diff --git a/gradlew.bat b/gradlew.bat new file mode 100644 index 00000000..6689b85b --- /dev/null +++ b/gradlew.bat @@ -0,0 +1,92 @@ +@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=. +@rem This is normally unused +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@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% equ 0 goto execute + +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 execute + +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 + +: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 %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 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! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/settings.gradle b/settings.gradle new file mode 100644 index 00000000..d94f73c6 --- /dev/null +++ b/settings.gradle @@ -0,0 +1,30 @@ +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2024' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } +} + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/src/main/deploy/README.md b/src/main/deploy/README.md new file mode 100644 index 00000000..d6ec5cf8 --- /dev/null +++ b/src/main/deploy/README.md @@ -0,0 +1,3 @@ +Files placed in this directory will be deployed to the RoboRIO into the +'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function +to get a proper path relative to the deploy directory. diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 00000000..ee6fb926 --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,79 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot; + +import edu.wpi.first.wpilibj.RobotBase; +import frc.robot.util.Alert; +import frc.robot.util.Alert.AlertType; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final double loopPeriodSecs = 0.02; + private static RobotType robotType = RobotType.COMPBOT; + public static final boolean tuningMode = false; + + public static RobotType getRobot() { + if (!disableHAL && RobotBase.isReal() && robotType == RobotType.SIMBOT) { + new Alert("Invalid robot selected, using competition robot as default.", AlertType.ERROR) + .set(true); + robotType = RobotType.COMPBOT; + } + return robotType; + } + + public static Mode getMode() { + return switch (robotType) { + case DEVBOT, COMPBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; + case SIMBOT -> Mode.SIM; + }; + } + + public static enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY + } + + public enum RobotType { + SIMBOT, + DEVBOT, + COMPBOT + } + + public static boolean disableHAL = false; + + public static void disableHAL() { + disableHAL = true; + } + + /** Checks whether the correct robot is selected when deploying. */ + public static void main(String... args) { + if (robotType == RobotType.SIMBOT) { + System.err.println("Cannot deploy, invalid robot selected: " + robotType); + System.exit(1); + } + } +} diff --git a/src/main/java/frc/robot/FieldConstants2024.java b/src/main/java/frc/robot/FieldConstants2024.java new file mode 100644 index 00000000..2d12811b --- /dev/null +++ b/src/main/java/frc/robot/FieldConstants2024.java @@ -0,0 +1,201 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// Copyright (c) 2024 FRC 2486 +// http://github.com/Coconuts2486-FRC +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot; + +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Filesystem; +import java.io.IOException; +import java.nio.file.Path; +import lombok.Getter; + +/** + * Contains various field dimensions and useful reference points. Dimensions are in meters, and sets + * of corners start in the lower left moving clockwise. All units in Meters
+ *
+ * + *

All translations and poses are stored with the origin at the rightmost point on the BLUE + * ALLIANCE wall.
+ *
+ * Length refers to the x direction (as described by wpilib)
+ * Width refers to the y direction (as described by wpilib) + */ +public class FieldConstants2024 { + public static final double fieldLength = Units.inchesToMeters(651.223); + public static final double fieldWidth = Units.inchesToMeters(323.277); + public static final double wingX = Units.inchesToMeters(229.201); + public static final double podiumX = Units.inchesToMeters(126.75); + public static final double startingLineX = Units.inchesToMeters(74.111); + + public static final Translation2d ampCenter = + new Translation2d(Units.inchesToMeters(72.455), fieldWidth); + + /** Staging locations for each note */ + public static final class StagingLocations { + public static final double centerlineX = fieldLength / 2.0; + + // need to update + public static final double centerlineFirstY = Units.inchesToMeters(29.638); + public static final double centerlineSeparationY = Units.inchesToMeters(66); + public static final double spikeX = Units.inchesToMeters(114); + // need + public static final double spikeFirstY = Units.inchesToMeters(161.638); + public static final double spikeSeparationY = Units.inchesToMeters(57); + + public static final Translation2d[] centerlineTranslations = new Translation2d[5]; + public static final Translation2d[] spikeTranslations = new Translation2d[3]; + + static { + for (int i = 0; i < centerlineTranslations.length; i++) { + centerlineTranslations[i] = + new Translation2d(centerlineX, centerlineFirstY + (i * centerlineSeparationY)); + } + } + + static { + for (int i = 0; i < spikeTranslations.length; i++) { + spikeTranslations[i] = new Translation2d(spikeX, spikeFirstY + (i * spikeSeparationY)); + } + } + } + + /** Each corner of the speaker * */ + public static final class Speaker { + + // corners (blue alliance origin) + public static final Translation3d topRightSpeaker = + new Translation3d( + Units.inchesToMeters(18.055), + Units.inchesToMeters(238.815), + Units.inchesToMeters(83.091)); + + public static final Translation3d topLeftSpeaker = + new Translation3d( + Units.inchesToMeters(18.055), + Units.inchesToMeters(197.765), + Units.inchesToMeters(83.091)); + + public static final Translation3d bottomRightSpeaker = + new Translation3d(0.0, Units.inchesToMeters(238.815), Units.inchesToMeters(78.324)); + public static final Translation3d bottomLeftSpeaker = + new Translation3d(0.0, Units.inchesToMeters(197.765), Units.inchesToMeters(78.324)); + + /** Center of the speaker opening (blue alliance) */ + public static final Translation3d centerSpeakerOpening = + bottomLeftSpeaker.interpolate(topRightSpeaker, 0.5); + } + + public static final class Subwoofer { + public static final Pose2d ampFaceCorner = + new Pose2d( + Units.inchesToMeters(35.775), + Units.inchesToMeters(239.366), + Rotation2d.fromDegrees(-120)); + + public static final Pose2d sourceFaceCorner = + new Pose2d( + Units.inchesToMeters(35.775), + Units.inchesToMeters(197.466), + Rotation2d.fromDegrees(120)); + + public static final Pose2d centerFace = + new Pose2d( + Units.inchesToMeters(35.775), + Units.inchesToMeters(218.416), + Rotation2d.fromDegrees(180)); + } + + public static final class Stage { + public static final Pose2d podiumLeg = + new Pose2d(Units.inchesToMeters(126.75), Units.inchesToMeters(161.638), new Rotation2d()); + public static final Pose2d ampLeg = + new Pose2d( + Units.inchesToMeters(220.873), + Units.inchesToMeters(212.425), + Rotation2d.fromDegrees(-30)); + public static final Pose2d sourceLeg = + new Pose2d( + Units.inchesToMeters(220.873), + Units.inchesToMeters(110.837), + Rotation2d.fromDegrees(30)); + + public static final Pose2d centerPodiumAmpChain = + new Pose2d( + podiumLeg.getTranslation().interpolate(ampLeg.getTranslation(), 0.5), + Rotation2d.fromDegrees(120.0)); + public static final Pose2d centerAmpSourceChain = + new Pose2d( + ampLeg.getTranslation().interpolate(sourceLeg.getTranslation(), 0.5), new Rotation2d()); + public static final Pose2d centerSourcePodiumChain = + new Pose2d( + sourceLeg.getTranslation().interpolate(podiumLeg.getTranslation(), 0.5), + Rotation2d.fromDegrees(240.0)); + public static final Pose2d center = + new Pose2d(Units.inchesToMeters(192.55), Units.inchesToMeters(161.638), new Rotation2d()); + public static final double centerToChainDistance = + center.getTranslation().getDistance(centerPodiumAmpChain.getTranslation()); + } + + public static final class Amp { + public static final Translation2d ampTapeTopCorner = + new Translation2d(Units.inchesToMeters(130.0), Units.inchesToMeters(305.256)); + public static final double ampBottomY = fieldWidth - Units.inchesToMeters(17.75); + } + + public static final double aprilTagWidth = Units.inchesToMeters(6.50); + public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; + + @Getter + public enum AprilTagLayoutType { + OFFICIAL("2024-official"), + SPEAKERS_ONLY("2024-speakers"), + AMPS_ONLY("2024-amps"), + WPI("2024-wpi"); + + private AprilTagLayoutType(String name) { + if (Constants.disableHAL) { + layout = null; + } else { + try { + layout = + new AprilTagFieldLayout( + Path.of(Filesystem.getDeployDirectory().getPath(), "apriltags", name + ".json")); + } catch (IOException e) { + throw new RuntimeException(e); + } + } + if (layout == null) { + layoutString = ""; + } else { + try { + layoutString = new ObjectMapper().writeValueAsString(layout); + } catch (JsonProcessingException e) { + throw new RuntimeException( + "Failed to serialize AprilTag layout JSON " + toString() + "for Northstar"); + } + } + } + + private final AprilTagFieldLayout layout; + private final String layoutString; + } +} diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java new file mode 100644 index 00000000..e8af1c10 --- /dev/null +++ b/src/main/java/frc/robot/Main.java @@ -0,0 +1,34 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java new file mode 100644 index 00000000..0c1c9f51 --- /dev/null +++ b/src/main/java/frc/robot/Robot.java @@ -0,0 +1,167 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.util.VirtualSubsystem; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends LoggedRobot { + private Command autonomousCommand; + private RobotContainer robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Record metadata + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + + // Set up data receivers & replay source + switch (Constants.getMode()) { + case REAL: + // Running on a real robot, log to a USB stick ("/U/logs") + Logger.addDataReceiver(new WPILOGWriter()); + Logger.addDataReceiver(new NT4Publisher()); + break; + + case SIM: + // Running a physics simulator, log to NT + Logger.addDataReceiver(new NT4Publisher()); + break; + + case REPLAY: + // Replaying a log, set up replay source + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); + break; + } + + // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. + // Logger.disableDeterministicTimestamps() + + // Start AdvantageKit logger; disable protobuf warnings + Logger.start(); + LogTable.disableProtobufWarning(); + + Logger.recordOutput("Cmd_Status/GP Tracking", false); + + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our autonomous chooser on the dashboard. + robotContainer = new RobotContainer(); + } + + /** This function is called periodically during all modes. */ + @Override + public void robotPeriodic() { + // Run all virtual subsystems each time through the loop + VirtualSubsystem.periodicAll(); + + // Runs the Scheduler. This is responsible for polling buttons, adding + // newly-scheduled commands, running already-scheduled commands, removing + // finished or interrupted commands, and running subsystem periodic() methods. + // This must be called from the robot's periodic block in order for anything in + // the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** This function is called once when the robot is disabled. */ + @Override + public void disabledInit() {} + + /** This function is called periodically when disabled. */ + @Override + public void disabledPeriodic() {} + + /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ + @Override + public void autonomousInit() { + autonomousCommand = robotContainer.getAutonomousCommand(); + + // schedule the autonomous command (example) + if (autonomousCommand != null) { + autonomousCommand.schedule(); + } + } + + /** This function is called periodically during autonomous. */ + @Override + public void autonomousPeriodic() {} + + /** This function is called once when teleop is enabled. */ + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (autonomousCommand != null) { + autonomousCommand.cancel(); + } + } + /** This function is called periodically during operator control. */ + @Override + public void teleopPeriodic() {} + + /** This function is called once when test mode is enabled. */ + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() {} + + /** This function is called once when the robot is first started up. */ + @Override + public void simulationInit() {} + + /** This function is called periodically whilst in simulation. */ + @Override + public void simulationPeriodic() {} +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java new file mode 100644 index 00000000..de2c9d09 --- /dev/null +++ b/src/main/java/frc/robot/RobotContainer.java @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; + +public class RobotContainer { + public RobotContainer() { + configureBindings(); + } + + private void configureBindings() {} + + public Command getAutonomousCommand() { + return Commands.print("No autonomous command configured"); + } +} diff --git a/src/main/java/frc/robot/util/Alert.java b/src/main/java/frc/robot/util/Alert.java new file mode 100644 index 00000000..28adb52a --- /dev/null +++ b/src/main/java/frc/robot/util/Alert.java @@ -0,0 +1,155 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// Copyright (c) 2024 FRC 2486 +// http://github.com/Coconuts2486-FRC +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.ArrayList; +import java.util.Comparator; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.function.Predicate; + +/** Class for managing persistent alerts to be sent over NetworkTables. */ +public class Alert { + private static Map groups = new HashMap(); + + private final AlertType type; + private boolean active = false; + private double activeStartTime = 0.0; + private String text; + + /** + * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, + * the appropriate entries will be added to NetworkTables. + * + * @param text Text to be displayed when the alert is active. + * @param type Alert level specifying urgency. + */ + public Alert(String text, AlertType type) { + this("Alerts", text, type); + } + + /** + * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate + * entries will be added to NetworkTables. + * + * @param group Group identifier, also used as NetworkTables title + * @param text Text to be displayed when the alert is active. + * @param type Alert level specifying urgency. + */ + public Alert(String group, String text, AlertType type) { + if (!groups.containsKey(group)) { + groups.put(group, new SendableAlerts()); + SmartDashboard.putData(group, groups.get(group)); + } + + this.text = text; + this.type = type; + groups.get(group).alerts.add(this); + } + + /** + * Sets whether the alert should currently be displayed. When activated, the alert text will also + * be sent to the console. + */ + public void set(boolean active) { + if (active && !this.active) { + activeStartTime = Timer.getFPGATimestamp(); + switch (type) { + case ERROR: + DriverStation.reportError(text, false); + break; + case WARNING: + DriverStation.reportWarning(text, false); + break; + case INFO: + System.out.println(text); + break; + } + } + this.active = active; + } + + /** Updates current alert text. */ + public void setText(String text) { + if (active && !text.equals(this.text)) { + switch (type) { + case ERROR: + DriverStation.reportError(text, false); + break; + case WARNING: + DriverStation.reportWarning(text, false); + break; + case INFO: + System.out.println(text); + break; + } + } + this.text = text; + } + + private static class SendableAlerts implements Sendable { + public final List alerts = new ArrayList<>(); + + public String[] getStrings(AlertType type) { + Predicate activeFilter = (Alert x) -> x.type == type && x.active; + Comparator timeSorter = + (Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime); + return alerts.stream() + .filter(activeFilter) + .sorted(timeSorter) + .map((Alert a) -> a.text) + .toArray(String[]::new); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Alerts"); + builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null); + builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null); + builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null); + } + } + + /** Represents an alert's level of urgency. */ + public static enum AlertType { + /** + * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type + * for problems which will seriously affect the robot's functionality and thus require immediate + * attention. + */ + ERROR, + + /** + * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this + * type for problems which could affect the robot's functionality but do not necessarily require + * immediate attention. + */ + WARNING, + + /** + * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type + * for problems which are unlikely to affect the robot's functionality, or any other alerts + * which do not fall under "ERROR" or "WARNING". + */ + INFO + } +} diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java new file mode 100644 index 00000000..7712de62 --- /dev/null +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -0,0 +1,105 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// Copyright (c) 2024 FRC 2486 +// http://github.com/Coconuts2486-FRC +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.FieldConstants2024; + +/** Utility functions for flipping from the blue to red alliance. */ +public class AllianceFlipUtil { + /** Flips an x coordinate to the correct side of the field based on the current alliance color. */ + public static double apply(double xCoordinate) { + if (shouldFlip()) { + return FieldConstants2024.fieldLength - xCoordinate; + } else { + return xCoordinate; + } + } + + /** Flips a translation to the correct side of the field based on the current alliance color. */ + public static Translation2d apply(Translation2d translation) { + if (shouldFlip()) { + return new Translation2d(apply(translation.getX()), translation.getY()); + } else { + return translation; + } + } + + /** Flips a rotation based on the current alliance color. */ + public static Rotation2d apply(Rotation2d rotation) { + if (shouldFlip()) { + return new Rotation2d(-rotation.getCos(), rotation.getSin()); + } else { + return rotation; + } + } + + /** Flips a pose to the correct side of the field based on the current alliance color. */ + public static Pose2d apply(Pose2d pose) { + if (shouldFlip()) { + return new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())); + } else { + return pose; + } + } + + public static Translation3d apply(Translation3d translation3d) { + if (shouldFlip()) { + return new Translation3d( + apply(translation3d.getX()), translation3d.getY(), translation3d.getZ()); + } else { + return translation3d; + } + } + + // /** + // * Flips a trajectory state to the correct side of the field based on the current alliance + // color. + // */ + // public static VehicleState apply(VehicleState state) { + // if (shouldFlip()) { + // return VehicleState.newBuilder() + // .setX(apply(state.getX())) + // .setY(state.getY()) + // .setTheta(apply(new Rotation2d(state.getTheta())).getRadians()) + // .setVx(-state.getVx()) + // .setVy(state.getVy()) + // .setOmega(-state.getOmega()) + // .addAllModuleForces( + // state.getModuleForcesList().stream() + // .map( + // forces -> + // VehicleTrajectoryServiceOuterClass.ModuleForce.newBuilder() + // .setFx(-forces.getFx()) + // .setFy(forces.getFy()) + // .build()) + // .toList()) + // .build(); + // } else { + // return state; + // } + // } + + public static boolean shouldFlip() { + return DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + } +} diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java new file mode 100644 index 00000000..4df60695 --- /dev/null +++ b/src/main/java/frc/robot/util/GeomUtil.java @@ -0,0 +1,171 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// Copyright (c) 2024 FRC 2486 +// http://github.com/Coconuts2486-FRC +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** Geometry utilities for working with translations, rotations, transforms, and poses. */ +public class GeomUtil { + /** + * Creates a pure translating transform + * + * @param translation The translation to create the transform with + * @return The resulting transform + */ + public static Transform2d toTransform2d(Translation2d translation) { + return new Transform2d(translation, new Rotation2d()); + } + + /** + * Creates a pure translating transform + * + * @param x The x coordinate of the translation + * @param y The y coordinate of the translation + * @return The resulting transform + */ + public static Transform2d toTransform2d(double x, double y) { + return new Transform2d(x, y, new Rotation2d()); + } + + /** + * Creates a pure rotating transform + * + * @param rotation The rotation to create the transform with + * @return The resulting transform + */ + public static Transform2d toTransform2d(Rotation2d rotation) { + return new Transform2d(new Translation2d(), rotation); + } + + /** + * Converts a Pose2d to a Transform2d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform2d toTransform2d(Pose2d pose) { + return new Transform2d(pose.getTranslation(), pose.getRotation()); + } + + public static Pose2d inverse(Pose2d pose) { + Rotation2d rotationInverse = pose.getRotation().unaryMinus(); + return new Pose2d( + pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse); + } + + /** + * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose2d toPose2d(Transform2d transform) { + return new Pose2d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Creates a pure translated pose + * + * @param translation The translation to create the pose with + * @return The resulting pose + */ + public static Pose2d toPose2d(Translation2d translation) { + return new Pose2d(translation, new Rotation2d()); + } + + /** + * Creates a pure rotated pose + * + * @param rotation The rotation to create the pose with + * @return The resulting pose + */ + public static Pose2d toPose2d(Rotation2d rotation) { + return new Pose2d(new Translation2d(), rotation); + } + + /** + * Multiplies a twist by a scaling factor + * + * @param twist The twist to multiply + * @param factor The scaling factor for the twist components + * @return The new twist + */ + public static Twist2d multiply(Twist2d twist, double factor) { + return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor); + } + + /** + * Converts a Pose3d to a Transform3d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform3d toTransform3d(Pose3d pose) { + return new Transform3d(pose.getTranslation(), pose.getRotation()); + } + + /** + * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose3d toPose3d(Transform3d transform) { + return new Pose3d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain + * + * @param speeds The original translation + * @return The resulting translation + */ + public static Twist2d toTwist2d(ChassisSpeeds speeds) { + return new Twist2d( + speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond); + } + + /** + * Creates a new pose from an existing one using a different translation value. + * + * @param pose The original pose + * @param translation The new translation to use + * @return The new pose with the new translation and original rotation + */ + public static Pose2d withTranslation(Pose2d pose, Translation2d translation) { + return new Pose2d(translation, pose.getRotation()); + } + + /** + * Creates a new pose from an existing one using a different rotation value. + * + * @param pose The original pose + * @param rotation The new rotation to use + * @return The new pose with the original translation and new rotation + */ + public static Pose2d withRotation(Pose2d pose, Rotation2d rotation) { + return new Pose2d(pose.getTranslation(), rotation); + } +} diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java new file mode 100644 index 00000000..191b54f2 --- /dev/null +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -0,0 +1,153 @@ +package frc.robot.util; + +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.pathfinding.LocalADStar; +import com.pathplanner.lib.pathfinding.Pathfinder; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Translation2d; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +// NOTE: This file is available at +// https://gist.github.com/mjansen4857/a8024b55eb427184dbd10ae8923bd57d + +public class LocalADStarAK implements Pathfinder { + private final ADStarIO io = new ADStarIO(); + + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } + + Logger.processInputs("LocalADStarAK", io); + + return io.isNewPathAvailable; + } + + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the pathfinder + */ + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } + + Logger.processInputs("LocalADStarAK", io); + + if (io.currentPathPoints.isEmpty()) { + return null; + } + + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } + + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an obstacle it will be + * moved to the nearest non-obstacle node. + */ + @Override + public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } + } + + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved + * to the nearest non-obstacle node. + */ + @Override + public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } + } + + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents + * opposite corners of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to change the start + * position of the path to properly avoid obstacles + */ + @Override + public void setDynamicObstacles( + List> obs, Translation2d currentRobotPos) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } + } + + private static class ADStarIO implements LoggableInputs { + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + + List pathPoints = new ArrayList<>(); + for (int i = 0; i < pointsLogged.length; i += 2) { + pathPoints.add( + new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } + } +} diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java new file mode 100644 index 00000000..d3ff834b --- /dev/null +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -0,0 +1,131 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// Copyright (c) 2024 FRC 2486 +// http://github.com/Coconuts2486-FRC +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util; + +import frc.robot.Constants; +import java.util.Arrays; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Consumer; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. + */ +public class LoggedTunableNumber implements DoubleSupplier { + private static final String tableKey = "TunableNumbers"; + + private final String key; + private boolean hasDefault = false; + private double defaultValue; + private LoggedDashboardNumber dashboardNumber; + private Map lastHasChangedValues = new HashMap<>(); + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey) { + this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(double defaultValue) { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + if (Constants.tuningMode) { + dashboardNumber = new LoggedDashboardNumber(key, defaultValue); + } + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public double get() { + if (!hasDefault) { + return 0.0; + } else { + return Constants.tuningMode ? dashboardNumber.get() : defaultValue; + } + } + + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false + * otherwise. + */ + public boolean hasChanged(int id) { + double currentValue = get(); + Double lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; + } + + return false; + } + + /** + * Runs action if any of the tunableNumbers have changed + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple * + * objects. Recommended approach is to pass the result of "hashCode()" + * @param action Callback to run when any of the tunable numbers have changed. Access tunable + * numbers in order inputted in method + * @param tunableNumbers All tunable numbers to check + */ + public static void ifChanged( + int id, Consumer action, LoggedTunableNumber... tunableNumbers) { + if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { + action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()); + } + } + + /** Runs action if any of the tunableNumbers have changed */ + public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { + ifChanged(id, values -> action.run(), tunableNumbers); + } + + @Override + public double getAsDouble() { + return get(); + } +} diff --git a/src/main/java/frc/robot/util/OverrideSwitches.java b/src/main/java/frc/robot/util/OverrideSwitches.java new file mode 100644 index 00000000..ef38b7a3 --- /dev/null +++ b/src/main/java/frc/robot/util/OverrideSwitches.java @@ -0,0 +1,92 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// Copyright (c) 2024 FRC 2486 +// http://github.com/Coconuts2486-FRC +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +/** Interface for physical override switches on operator console. */ +public class OverrideSwitches { + private final GenericHID joystick; + + public OverrideSwitches(int port) { + joystick = new GenericHID(port); + } + + /** Returns whether the controller is connected. */ + public boolean isConnected() { + return joystick.isConnected() + && !DriverStation.getJoystickIsXbox(joystick.getPort()) + && joystick.getName().equals("Generic USB Joystick"); + } + + /** Gets the state of a driver-side switch (0-2 from left to right). */ + public boolean getDriverSwitch(int index) { + if (index < 0 || index > 2) { + throw new RuntimeException( + "Invalid driver override index " + Integer.toString(index) + ". Must be 0-2."); + } + return joystick.getRawButton(index + 1); + } + + /** Gets the state of an operator-side switch (0-4 from left to right). */ + public boolean getOperatorSwitch(int index) { + if (index < 0 || index > 4) { + throw new RuntimeException( + "Invalid operator override index " + Integer.toString(index) + ". Must be 0-4."); + } + return joystick.getRawButton(index + 8); + } + + /** Gets the state of the multi-directional switch. */ + public MultiDirectionSwitchState getMultiDirectionSwitch() { + if (joystick.getRawButton(4)) { + return MultiDirectionSwitchState.LEFT; + } else if (joystick.getRawButton(5)) { + return MultiDirectionSwitchState.RIGHT; + } else { + return MultiDirectionSwitchState.NEUTRAL; + } + } + + /** Returns a trigger for a driver-side switch (0-2 from left to right). */ + public Trigger driverSwitch(int index) { + return new Trigger(() -> getDriverSwitch(index)); + } + + /** Returns a trigger for an operator-side switch (0-4 from left to right). */ + public Trigger operatorSwitch(int index) { + return new Trigger(() -> getOperatorSwitch(index)); + } + + /** Returns a trigger for when the multi-directional switch is pushed to the left. */ + public Trigger multiDirectionSwitchLeft() { + return new Trigger(() -> getMultiDirectionSwitch() == MultiDirectionSwitchState.LEFT); + } + + /** Returns a trigger for when the multi-directional switch is pushed to the right. */ + public Trigger multiDirectionSwitchRight() { + return new Trigger(() -> getMultiDirectionSwitch() == MultiDirectionSwitchState.RIGHT); + } + + /** The state of the multi-directional switch. */ + public static enum MultiDirectionSwitchState { + LEFT, + NEUTRAL, + RIGHT + } +} diff --git a/src/main/java/frc/robot/util/VirtualSubsystem.java b/src/main/java/frc/robot/util/VirtualSubsystem.java new file mode 100644 index 00000000..55f2cdd5 --- /dev/null +++ b/src/main/java/frc/robot/util/VirtualSubsystem.java @@ -0,0 +1,38 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// Copyright (c) 2024 FRC 2486 +// http://github.com/Coconuts2486-FRC +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util; + +import java.util.ArrayList; +import java.util.List; + +/** + * Base class for virtual subsystems -- not robot hardware -- that should be treated as subsystems + */ +public abstract class VirtualSubsystem { + private static List subsystems = new ArrayList<>(); + + public VirtualSubsystem() { + subsystems.add(this); + } + + public static void periodicAll() { + for (VirtualSubsystem subsystem : subsystems) { + subsystem.periodic(); + } + } + + public abstract void periodic(); +} diff --git a/src/main/java/frc/robot/util/swerve/ModuleLimits.java b/src/main/java/frc/robot/util/swerve/ModuleLimits.java new file mode 100644 index 00000000..b08e4a8c --- /dev/null +++ b/src/main/java/frc/robot/util/swerve/ModuleLimits.java @@ -0,0 +1,13 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// Copyright (c) 2024 FRC 2486 +// http://github.com/Coconuts2486-FRC +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util.swerve; + +public record ModuleLimits( + double maxDriveVelocity, double maxDriveAcceleration, double maxSteeringVelocity) {} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 00000000..1f57c3b6 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,42 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "3.2.1", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2024", + "mavenUrls": [], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "wpilib-shim", + "version": "3.2.1" + }, + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "junction-core", + "version": "3.2.1" + }, + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-api", + "version": "3.2.1" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-wpilibio", + "version": "3.2.1", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [] +} diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 00000000..f528a51a --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2024.2.8", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2024", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2024.2.8" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2024.2.8", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 00000000..adf6e02a --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.33.1", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.33.1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.33.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.33.1", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.33.1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.33.1", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.33.1", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.1", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json new file mode 100644 index 00000000..1f0cf3ab --- /dev/null +++ b/vendordeps/Phoenix6.json @@ -0,0 +1,339 @@ +{ + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.3.0", + "frcYear": 2024, + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "conflictsWith": [ + { + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "24.3.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "24.3.0", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.3.0", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "24.3.0", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.3.0", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.3.0", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.3.0", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.3.0", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.3.0", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.3.0", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.3.0", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.3.0", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.3.0", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 00000000..9eda5acb --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.4", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.2.4" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.4", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.2.4", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.4", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json new file mode 100644 index 00000000..67bf3898 --- /dev/null +++ b/vendordeps/WPILibNewCommands.json @@ -0,0 +1,38 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "1.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2024", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 00000000..bef469ee --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,57 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2024.3.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2024.3.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2024.3.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2024.3.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2024.3.1" + } + ] +} diff --git a/vendordeps/yagsl.json b/vendordeps/yagsl.json new file mode 100644 index 00000000..9a4905ee --- /dev/null +++ b/vendordeps/yagsl.json @@ -0,0 +1,20 @@ +{ + "fileName": "yagsl.json", + "name": "YAGSL", + "version": "2024.5.0.4", + "frcYear": "2024", + "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", + "mavenUrls": [ + "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/repos" + ], + "jsonUrl": "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/yagsl.json", + "javaDependencies": [ + { + "groupId": "swervelib", + "artifactId": "YAGSL-java", + "version": "2024.5.0.4" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} From b70421047932238c0da57e2c3d82d33c51f263a9 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Mon, 23 Sep 2024 13:36:40 -0700 Subject: [PATCH 03/57] Adding from AdvantageKit's Advanced Swerve sketch Plus, updating README to provide some basic information and playing around with the copyrights to point to Az-FIRST. modified: README.md new file: src/main/deploy/pathplanner2024/autos/Example Auto.auto new file: src/main/deploy/pathplanner2024/navgrid.json new file: src/main/deploy/pathplanner2024/paths/Example Path.path new file: src/main/java/frc/robot/commands/DriveCommands.java new file: src/main/java/frc/robot/subsystems/drive/Drive.java new file: src/main/java/frc/robot/subsystems/drive/GyroIO.java new file: src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java new file: src/main/java/frc/robot/subsystems/drive/Module.java new file: src/main/java/frc/robot/subsystems/drive/ModuleIO.java new file: src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java new file: src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java new file: src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java new file: src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java new file: src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java new file: src/main/java/frc/robot/subsystems/flywheel/Flywheel.java new file: src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java new file: src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java new file: src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java new file: src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java modified: src/main/java/frc/robot/util/Alert.java modified: src/main/java/frc/robot/util/AllianceFlipUtil.java modified: src/main/java/frc/robot/util/GeomUtil.java modified: src/main/java/frc/robot/util/LocalADStarAK.java modified: src/main/java/frc/robot/util/LoggedTunableNumber.java modified: src/main/java/frc/robot/util/OverrideSwitches.java modified: src/main/java/frc/robot/util/VirtualSubsystem.java modified: src/main/java/frc/robot/util/swerve/ModuleLimits.java --- README.md | 32 + .../pathplanner2024/autos/Example Auto.auto | 31 + src/main/deploy/pathplanner2024/navgrid.json | 1633 +++++++++++++++++ .../pathplanner2024/paths/Example Path.path | 65 + .../frc/robot/commands/DriveCommands.java | 77 + .../frc/robot/subsystems/drive/Drive.java | 299 +++ .../frc/robot/subsystems/drive/GyroIO.java | 30 + .../robot/subsystems/drive/GyroIOPigeon2.java | 75 + .../frc/robot/subsystems/drive/Module.java | 204 ++ .../frc/robot/subsystems/drive/ModuleIO.java | 52 + .../robot/subsystems/drive/ModuleIOSim.java | 72 + .../subsystems/drive/ModuleIOSparkMax.java | 202 ++ .../subsystems/drive/ModuleIOTalonFX.java | 218 +++ .../drive/PhoenixOdometryThread.java | 138 ++ .../drive/SparkMaxOdometryThread.java | 107 ++ .../robot/subsystems/flywheel/Flywheel.java | 110 ++ .../robot/subsystems/flywheel/FlywheelIO.java | 41 + .../subsystems/flywheel/FlywheelIOSim.java | 68 + .../flywheel/FlywheelIOSparkMax.java | 89 + .../flywheel/FlywheelIOTalonFX.java | 98 + src/main/java/frc/robot/util/Alert.java | 4 +- .../java/frc/robot/util/AllianceFlipUtil.java | 4 +- src/main/java/frc/robot/util/GeomUtil.java | 4 +- .../java/frc/robot/util/LocalADStarAK.java | 16 + .../frc/robot/util/LoggedTunableNumber.java | 4 +- .../java/frc/robot/util/OverrideSwitches.java | 22 +- .../java/frc/robot/util/VirtualSubsystem.java | 7 +- .../frc/robot/util/swerve/ModuleLimits.java | 16 +- 28 files changed, 3692 insertions(+), 26 deletions(-) create mode 100644 src/main/deploy/pathplanner2024/autos/Example Auto.auto create mode 100644 src/main/deploy/pathplanner2024/navgrid.json create mode 100644 src/main/deploy/pathplanner2024/paths/Example Path.path create mode 100644 src/main/java/frc/robot/commands/DriveCommands.java create mode 100644 src/main/java/frc/robot/subsystems/drive/Drive.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIO.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java create mode 100644 src/main/java/frc/robot/subsystems/drive/Module.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIO.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java create mode 100644 src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java create mode 100644 src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java create mode 100644 src/main/java/frc/robot/subsystems/flywheel/Flywheel.java create mode 100644 src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java create mode 100644 src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java create mode 100644 src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java diff --git a/README.md b/README.md index 371a25dc..2e720d93 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,34 @@ # Az-RBSI Arizona's Reference Build and Software Implementation for FRC Robots (read: "A-Z-ribsy") + +## Purpose + +The purpose of Az-RBSI is to help Arizona FRC teams with: +* Improving robot reliability / performance during “Autonomous Play” +* Improving robot build & endurance, gameplay reliability and troubleshooting + skills +* Providing a standardized robot “stack” to allow for quick software setup and + troubleshooting, and make it easier for Arizona teams to form effective + in-state alliances + + +## Design Philosophy + +The Az-RBSI is centered around a "Reference Build" robot that allows for teams +to communicate quickly and effectivly with each other about gameplay strategy +and troubleshooting. Additionally, the consolidation around a standard robot +design allows for easier swapping of spare parts and programming modules. + +The Az-RBSI software is an outline of an FRC robot program upon which teams can +build with their particular mechanisms and designs. It weaves together popular +and currently maintained FIRST- and community-sponsored software libraries to +provide a baseline robot functionality that combines robust reliability with +effective logging for troubleshooting. + +## Library Dependencies + +* [WPILib](https://docs.wpilib.org/en/stable/index.html) +* [AdvantageKit](https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/docs/WHAT-IS-ADVANTAGEKIT.md) +* [YAGSL](https://yagsl.gitbook.io/yagsl) +* [PathPlanner](https://pathplanner.dev/home.html) +* [PhotonVision](https://docs.photonvision.org/en/latest/) diff --git a/src/main/deploy/pathplanner2024/autos/Example Auto.auto b/src/main/deploy/pathplanner2024/autos/Example Auto.auto new file mode 100644 index 00000000..77a8433f --- /dev/null +++ b/src/main/deploy/pathplanner2024/autos/Example Auto.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.0, + "y": 7.0 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Run Flywheel" + } + }, + { + "type": "path", + "data": { + "pathName": "Example Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner2024/navgrid.json b/src/main/deploy/pathplanner2024/navgrid.json new file mode 100644 index 00000000..690f5db2 --- /dev/null +++ b/src/main/deploy/pathplanner2024/navgrid.json @@ -0,0 +1,1633 @@ +{ + "field_size": { + "x": 16.54, + "y": 8.21 + }, + "nodeSizeMeters": 0.3, + "grid": [ + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true + ], + [ + true, + true, + true, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ] + ] +} diff --git a/src/main/deploy/pathplanner2024/paths/Example Path.path b/src/main/deploy/pathplanner2024/paths/Example Path.path new file mode 100644 index 00000000..8f3609bb --- /dev/null +++ b/src/main/deploy/pathplanner2024/paths/Example Path.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.013282910175676, + "y": 6.5274984191074985 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.166769560390973, + "y": 5.0964860911203305 + }, + "prevControl": { + "x": 4.166769560390973, + "y": 6.0964860911203305 + }, + "nextControl": { + "x": 6.166769560390973, + "y": 4.0964860911203305 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0, + "y": 1.0 + }, + "prevControl": { + "x": 6.75, + "y": 2.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java new file mode 100644 index 00000000..fc8f596b --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -0,0 +1,77 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.drive.Drive; +import java.util.function.DoubleSupplier; + +public class DriveCommands { + private static final double DEADBAND = 0.1; + + private DriveCommands() {} + + /** + * Field relative drive command using two joysticks (controlling linear and angular velocities). + */ + public static Command joystickDrive( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier) { + return Commands.run( + () -> { + // Apply deadband + double linearMagnitude = + MathUtil.applyDeadband( + Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND); + Rotation2d linearDirection = + new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); + + // Square values + linearMagnitude = linearMagnitude * linearMagnitude; + omega = Math.copySign(omega * omega, omega); + + // Calcaulate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + + // Convert to field relative speeds & send command + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega * drive.getMaxAngularSpeedRadPerSec(), + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); + }, + drive); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java new file mode 100644 index 00000000..51d02c0e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -0,0 +1,299 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import static edu.wpi.first.units.Units.*; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.pathfinding.Pathfinding; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PathPlannerLogging; +import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.util.LocalADStarAK; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class Drive extends SubsystemBase { + private static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5); + private static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); + private static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); + private static final double DRIVE_BASE_RADIUS = + Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); + private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; + + static final Lock odometryLock = new ReentrantLock(); + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final Module[] modules = new Module[4]; // FL, FR, BL, BR + private final SysIdRoutine sysId; + + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); + private Rotation2d rawGyroRotation = new Rotation2d(); + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + private SwerveDrivePoseEstimator poseEstimator = + new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); + + public Drive( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO) { + this.gyroIO = gyroIO; + modules[0] = new Module(flModuleIO, 0); + modules[1] = new Module(frModuleIO, 1); + modules[2] = new Module(blModuleIO, 2); + modules[3] = new Module(brModuleIO, 3); + + // Start threads (no-op for each if no signals have been created) + PhoenixOdometryThread.getInstance().start(); + SparkMaxOdometryThread.getInstance().start(); + + // Configure AutoBuilder for PathPlanner + AutoBuilder.configureHolonomic( + this::getPose, + this::setPose, + () -> kinematics.toChassisSpeeds(getModuleStates()), + this::runVelocity, + new HolonomicPathFollowerConfig( + MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), + () -> + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red, + this); + Pathfinding.setPathfinder(new LocalADStarAK()); + PathPlannerLogging.setLogActivePathCallback( + (activePath) -> { + Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + }); + PathPlannerLogging.setLogTargetPoseCallback( + (targetPose) -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); + + // Configure SysId + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> { + for (int i = 0; i < 4; i++) { + modules[i].runCharacterization(voltage.in(Volts)); + } + }, + null, + this)); + } + + public void periodic() { + odometryLock.lock(); // Prevents odometry updates while reading data + gyroIO.updateInputs(gyroInputs); + for (var module : modules) { + module.updateInputs(); + } + odometryLock.unlock(); + Logger.processInputs("Drive/Gyro", gyroInputs); + for (var module : modules) { + module.periodic(); + } + + // Stop moving when disabled + if (DriverStation.isDisabled()) { + for (var module : modules) { + module.stop(); + } + } + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + } + + // Update odometry + double[] sampleTimestamps = + modules[0].getOdometryTimestamps(); // All signals are sampled together + int sampleCount = sampleTimestamps.length; + for (int i = 0; i < sampleCount; i++) { + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; + moduleDeltas[moduleIndex] = + new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters + - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + + // Update gyro angle + if (gyroInputs.connected) { + // Use the real gyro angle + rawGyroRotation = gyroInputs.odometryYawPositions[i]; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + } + + // Apply update + poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + } + } + + /** + * Runs the drive at the desired velocity. + * + * @param speeds Speeds in meters/sec + */ + public void runVelocity(ChassisSpeeds speeds) { + // Calculate module setpoints + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED); + + // Send setpoints to modules + SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + // The module returns the optimized state, useful for logging + optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); + } + + // Log setpoint states + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); + } + + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + /** + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will + * return to their normal orientations the next time a nonzero velocity is requested. + */ + public void stopWithX() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = getModuleTranslations()[i].getAngle(); + } + kinematics.resetHeadings(headings); + stop(); + } + + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysId.quasistatic(direction); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysId.dynamic(direction); + } + + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getState(); + } + return states; + } + + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getPosition(); + } + return states; + } + + /** Returns the current odometry pose. */ + @AutoLogOutput(key = "Odometry/Robot") + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** Returns the current odometry rotation. */ + public Rotation2d getRotation() { + return getPose().getRotation(); + } + + /** Resets the current odometry pose. */ + public void setPose(Pose2d pose) { + poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + } + + /** + * Adds a vision measurement to the pose estimator. + * + * @param visionPose The pose of the robot as measured by the vision camera. + * @param timestamp The timestamp of the vision measurement in seconds. + */ + public void addVisionMeasurement(Pose2d visionPose, double timestamp) { + poseEstimator.addVisionMeasurement(visionPose, timestamp); + } + + /** Returns the maximum linear speed in meters per sec. */ + public double getMaxLinearSpeedMetersPerSec() { + return MAX_LINEAR_SPEED; + } + + /** Returns the maximum angular speed in radians per sec. */ + public double getMaxAngularSpeedRadPerSec() { + return MAX_ANGULAR_SPEED; + } + + /** Returns an array of module translations. */ + public static Translation2d[] getModuleTranslations() { + return new Translation2d[] { + new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), + new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0), + new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), + new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0) + }; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java new file mode 100644 index 00000000..18ce6fde --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -0,0 +1,30 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface GyroIO { + @AutoLog + public static class GyroIOInputs { + public boolean connected = false; + public Rotation2d yawPosition = new Rotation2d(); + public double[] odometryYawTimestamps = new double[] {}; + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + public double yawVelocityRadPerSec = 0.0; + } + + public default void updateInputs(GyroIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java new file mode 100644 index 00000000..01ccef28 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -0,0 +1,75 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import java.util.OptionalDouble; +import java.util.Queue; + +/** IO implementation for Pigeon2 */ +public class GyroIOPigeon2 implements GyroIO { + private final Pigeon2 pigeon = new Pigeon2(20); + private final StatusSignal yaw = pigeon.getYaw(); + private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + + public GyroIOPigeon2(boolean phoenixDrive) { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(Module.ODOMETRY_FREQUENCY); + yawVelocity.setUpdateFrequency(100.0); + pigeon.optimizeBusUtilization(); + if (phoenixDrive) { + yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(pigeon, pigeon.getYaw()); + } else { + yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal( + () -> { + boolean valid = yaw.refresh().getStatus().isOK(); + if (valid) { + return OptionalDouble.of(yaw.getValueAsDouble()); + } else { + return OptionalDouble.empty(); + } + }); + } + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + + inputs.odometryYawTimestamps = + yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryYawPositions = + yawPositionQueue.stream() + .map((Double value) -> Rotation2d.fromDegrees(value)) + .toArray(Rotation2d[]::new); + yawTimestampQueue.clear(); + yawPositionQueue.clear(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java new file mode 100644 index 00000000..08eef971 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -0,0 +1,204 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; +import org.littletonrobotics.junction.Logger; + +public class Module { + private static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); + static final double ODOMETRY_FREQUENCY = 250.0; + + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + private final int index; + + private final SimpleMotorFeedforward driveFeedforward; + private final PIDController driveFeedback; + private final PIDController turnFeedback; + private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop + private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop + private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; + + public Module(ModuleIO io, int index) { + this.io = io; + this.index = index; + + // Switch constants based on mode (the physics simulator is treated as a + // separate robot with different tuning) + switch (Constants.getMode()) { + case REAL: + case REPLAY: + driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); + driveFeedback = new PIDController(0.05, 0.0, 0.0); + turnFeedback = new PIDController(7.0, 0.0, 0.0); + break; + case SIM: + driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); + driveFeedback = new PIDController(0.1, 0.0, 0.0); + turnFeedback = new PIDController(10.0, 0.0, 0.0); + break; + default: + driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); + driveFeedback = new PIDController(0.0, 0.0, 0.0); + turnFeedback = new PIDController(0.0, 0.0, 0.0); + break; + } + + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); + setBrakeMode(true); + } + + /** + * Update inputs without running the rest of the periodic logic. This is useful since these + * updates need to be properly thread-locked. + */ + public void updateInputs() { + io.updateInputs(inputs); + } + + public void periodic() { + Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); + + // On first cycle, reset relative turn encoder + // Wait until absolute angle is nonzero in case it wasn't initialized yet + if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { + turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); + } + + // Run closed loop turn control + if (angleSetpoint != null) { + io.setTurnVoltage( + turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); + + // Run closed loop drive control + // Only allowed if closed loop turn control is running + if (speedSetpoint != null) { + // Scale velocity based on turn error + // + // When the error is 90°, the velocity setpoint should be 0. As the wheel turns + // towards the setpoint, its velocity should increase. This is achieved by + // taking the component of the velocity in the direction of the setpoint. + double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); + + // Run drive controller + double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; + io.setDriveVoltage( + driveFeedforward.calculate(velocityRadPerSec) + + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); + } + } + + // Calculate positions for odometry + int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together + odometryPositions = new SwerveModulePosition[sampleCount]; + for (int i = 0; i < sampleCount; i++) { + double positionMeters = inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS; + Rotation2d angle = + inputs.odometryTurnPositions[i].plus( + turnRelativeOffset != null ? turnRelativeOffset : new Rotation2d()); + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + } + } + + /** Runs the module with the specified setpoint state. Returns the optimized state. */ + public SwerveModuleState runSetpoint(SwerveModuleState state) { + // Optimize state based on current angle + // Controllers run in "periodic" when the setpoint is not null + var optimizedState = SwerveModuleState.optimize(state, getAngle()); + + // Update setpoints, controllers run in "periodic" + angleSetpoint = optimizedState.angle; + speedSetpoint = optimizedState.speedMetersPerSecond; + + return optimizedState; + } + + /** Runs the module with the specified voltage while controlling to zero degrees. */ + public void runCharacterization(double volts) { + // Closed loop turn control + angleSetpoint = new Rotation2d(); + + // Open loop drive control + io.setDriveVoltage(volts); + speedSetpoint = null; + } + + /** Disables all outputs to motors. */ + public void stop() { + io.setTurnVoltage(0.0); + io.setDriveVoltage(0.0); + + // Disable closed loop control for turn and drive + angleSetpoint = null; + speedSetpoint = null; + } + + /** Sets whether brake mode is enabled. */ + public void setBrakeMode(boolean enabled) { + io.setDriveBrakeMode(enabled); + io.setTurnBrakeMode(enabled); + } + + /** Returns the current turn angle of the module. */ + public Rotation2d getAngle() { + if (turnRelativeOffset == null) { + return new Rotation2d(); + } else { + return inputs.turnPosition.plus(turnRelativeOffset); + } + } + + /** Returns the current drive position of the module in meters. */ + public double getPositionMeters() { + return inputs.drivePositionRad * WHEEL_RADIUS; + } + + /** Returns the current drive velocity of the module in meters per second. */ + public double getVelocityMetersPerSec() { + return inputs.driveVelocityRadPerSec * WHEEL_RADIUS; + } + + /** Returns the module position (turn angle and drive position). */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } + + /** Returns the module state (turn angle and drive velocity). */ + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + } + + /** Returns the module positions received this cycle. */ + public SwerveModulePosition[] getOdometryPositions() { + return odometryPositions; + } + + /** Returns the timestamps of the samples received this cycle. */ + public double[] getOdometryTimestamps() { + return inputs.odometryTimestamps; + } + + /** Returns the drive velocity in radians/sec. */ + public double getCharacterizationVelocity() { + return inputs.driveVelocityRadPerSec; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java new file mode 100644 index 00000000..200afa3a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -0,0 +1,52 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface ModuleIO { + @AutoLog + public static class ModuleIOInputs { + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double[] driveCurrentAmps = new double[] {}; + + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double[] turnCurrentAmps = new double[] {}; + + public double[] odometryTimestamps = new double[] {}; + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} + + /** Run the drive motor at the specified voltage. */ + public default void setDriveVoltage(double volts) {} + + /** Run the turn motor at the specified voltage. */ + public default void setTurnVoltage(double volts) {} + + /** Enable or disable brake mode on the drive motor. */ + public default void setDriveBrakeMode(boolean enable) {} + + /** Enable or disable brake mode on the turn motor. */ + public default void setTurnBrakeMode(boolean enable) {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java new file mode 100644 index 00000000..f2ffe480 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -0,0 +1,72 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +/** + * Physics sim implementation of module IO. + * + *

Uses two flywheel sims for the drive and turn motors, with the absolute position initialized + * to a random value. The flywheel sims are not physically accurate, but provide a decent + * approximation for the behavior of the module. + */ +public class ModuleIOSim implements ModuleIO { + private static final double LOOP_PERIOD_SECS = 0.02; + + private DCMotorSim driveSim = new DCMotorSim(DCMotor.getNEO(1), 6.75, 0.025); + private DCMotorSim turnSim = new DCMotorSim(DCMotor.getNEO(1), 150.0 / 7.0, 0.004); + + private final Rotation2d turnAbsoluteInitPosition = new Rotation2d(Math.random() * 2.0 * Math.PI); + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; + + @Override + public void updateInputs(ModuleIOInputs inputs) { + driveSim.update(LOOP_PERIOD_SECS); + turnSim.update(LOOP_PERIOD_SECS); + + inputs.drivePositionRad = driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; + + inputs.turnAbsolutePosition = + new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); + inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; + + inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; + inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; + inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; + } + + @Override + public void setDriveVoltage(double volts) { + driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + driveSim.setInputVoltage(driveAppliedVolts); + } + + @Override + public void setTurnVoltage(double volts) { + turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + turnSim.setInputVoltage(turnAppliedVolts); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java new file mode 100644 index 00000000..781ec70b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -0,0 +1,202 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkLowLevel.PeriodicFrame; +import com.revrobotics.CANSparkMax; +import com.revrobotics.REVLibError; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.RobotController; +import java.util.OptionalDouble; +import java.util.Queue; + +/** + * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO + * or NEO 550), and analog absolute encoder connected to the RIO + * + *

NOTE: This implementation should be used as a starting point and adapted to different hardware + * configurations (e.g. If using a CANcoder, copy from "ModuleIOTalonFX") + * + *

To calibrate the absolute encoder offsets, point the modules straight (such that forward + * motion on the drive motor will propel the robot forward) and copy the reported values from the + * absolute encoders using AdvantageScope. These values are logged under + * "/Drive/ModuleX/TurnAbsolutePositionRad" + */ +public class ModuleIOSparkMax implements ModuleIO { + // Gear ratios for SDS MK4i L2, adjust as necessary + private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + private static final double TURN_GEAR_RATIO = 150.0 / 7.0; + + private final CANSparkMax driveSparkMax; + private final CANSparkMax turnSparkMax; + + private final RelativeEncoder driveEncoder; + private final RelativeEncoder turnRelativeEncoder; + private final AnalogInput turnAbsoluteEncoder; + private final Queue timestampQueue; + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; + + public ModuleIOSparkMax(int index) { + switch (index) { + case 0: + driveSparkMax = new CANSparkMax(1, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(2, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(0); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 1: + driveSparkMax = new CANSparkMax(3, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(4, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(1); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 2: + driveSparkMax = new CANSparkMax(5, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(6, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(2); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 3: + driveSparkMax = new CANSparkMax(7, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(8, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(3); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + default: + throw new RuntimeException("Invalid module index"); + } + + driveSparkMax.restoreFactoryDefaults(); + turnSparkMax.restoreFactoryDefaults(); + + driveSparkMax.setCANTimeout(250); + turnSparkMax.setCANTimeout(250); + + driveEncoder = driveSparkMax.getEncoder(); + turnRelativeEncoder = turnSparkMax.getEncoder(); + + turnSparkMax.setInverted(isTurnMotorInverted); + driveSparkMax.setSmartCurrentLimit(40); + turnSparkMax.setSmartCurrentLimit(30); + driveSparkMax.enableVoltageCompensation(12.0); + turnSparkMax.enableVoltageCompensation(12.0); + + driveEncoder.setPosition(0.0); + driveEncoder.setMeasurementPeriod(10); + driveEncoder.setAverageDepth(2); + + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod(10); + turnRelativeEncoder.setAverageDepth(2); + + driveSparkMax.setCANTimeout(0); + turnSparkMax.setCANTimeout(0); + + driveSparkMax.setPeriodicFramePeriod( + PeriodicFrame.kStatus2, (int) (1000.0 / Module.ODOMETRY_FREQUENCY)); + turnSparkMax.setPeriodicFramePeriod( + PeriodicFrame.kStatus2, (int) (1000.0 / Module.ODOMETRY_FREQUENCY)); + timestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); + drivePositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal( + () -> { + double value = driveEncoder.getPosition(); + if (driveSparkMax.getLastError() == REVLibError.kOk) { + return OptionalDouble.of(value); + } else { + return OptionalDouble.empty(); + } + }); + turnPositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal( + () -> { + double value = turnRelativeEncoder.getPosition(); + if (turnSparkMax.getLastError() == REVLibError.kOk) { + return OptionalDouble.of(value); + } else { + return OptionalDouble.empty(); + } + }); + + driveSparkMax.burnFlash(); + turnSparkMax.burnFlash(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + inputs.drivePositionRad = + Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO; + inputs.driveVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO; + inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); + inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()}; + + inputs.turnAbsolutePosition = + new Rotation2d( + turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V() * 2.0 * Math.PI) + .minus(absoluteEncoderOffset); + inputs.turnPosition = + Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) + / TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; + + inputs.odometryTimestamps = + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = + drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream() + .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveSparkMax.setVoltage(volts); + } + + @Override + public void setTurnVoltage(double volts) { + turnSparkMax.setVoltage(volts); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java new file mode 100644 index 00000000..b41202ef --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -0,0 +1,218 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import java.util.Queue; + +/** + * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and + * CANcoder + * + *

NOTE: This implementation should be used as a starting point and adapted to different hardware + * configurations (e.g. If using an analog encoder, copy from "ModuleIOSparkMax") + * + *

To calibrate the absolute encoder offsets, point the modules straight (such that forward + * motion on the drive motor will propel the robot forward) and copy the reported values from the + * absolute encoders using AdvantageScope. These values are logged under + * "/Drive/ModuleX/TurnAbsolutePositionRad" + */ +public class ModuleIOTalonFX implements ModuleIO { + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final CANcoder cancoder; + + private final Queue timestampQueue; + + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final Queue turnPositionQueue; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Gear ratios for SDS MK4i L2, adjust as necessary + private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + private final double TURN_GEAR_RATIO = 150.0 / 7.0; + + private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; + + public ModuleIOTalonFX(int index) { + switch (index) { + case 0: + driveTalon = new TalonFX(0); + turnTalon = new TalonFX(1); + cancoder = new CANcoder(2); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 1: + driveTalon = new TalonFX(3); + turnTalon = new TalonFX(4); + cancoder = new CANcoder(5); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 2: + driveTalon = new TalonFX(6); + turnTalon = new TalonFX(7); + cancoder = new CANcoder(8); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 3: + driveTalon = new TalonFX(9); + turnTalon = new TalonFX(10); + cancoder = new CANcoder(11); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + default: + throw new RuntimeException("Invalid module index"); + } + + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveTalon.getConfigurator().apply(driveConfig); + setDriveBrakeMode(true); + + var turnConfig = new TalonFXConfiguration(); + turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; + turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + turnTalon.getConfigurator().apply(turnConfig); + setTurnBrakeMode(true); + + cancoder.getConfigurator().apply(new CANcoderConfiguration()); + + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + + drivePosition = driveTalon.getPosition(); + drivePositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getSupplyCurrent(); + + turnAbsolutePosition = cancoder.getAbsolutePosition(); + turnPosition = turnTalon.getPosition(); + turnPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnCurrent = turnTalon.getSupplyCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + Module.ODOMETRY_FREQUENCY, drivePosition, turnPosition); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + driveTalon.optimizeBusUtilization(); + turnTalon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + + inputs.drivePositionRad = + Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; + inputs.driveVelocityRadPerSec = + Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; + + inputs.turnAbsolutePosition = + Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()) + .minus(absoluteEncoderOffset); + inputs.turnPosition = + Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO); + inputs.turnVelocityRadPerSec = + Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; + + inputs.odometryTimestamps = + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = + drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream() + .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setTurnVoltage(double volts) { + turnTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + driveTalon.getConfigurator().apply(config); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = + isTurnMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + turnTalon.getConfigurator().apply(config); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java new file mode 100644 index 00000000..b7531fac --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -0,0 +1,138 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.ParentDevice; +import java.util.ArrayList; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import org.littletonrobotics.junction.Logger; + +/** + * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. + * + *

This version is intended for Phoenix 6 devices on both the RIO and CANivore buses. When using + * a CANivore, the thread uses the "waitForAll" blocking method to enable more consistent sampling. + * This also allows Phoenix Pro users to benefit from lower latency between devices using CANivore + * time synchronization. + */ +public class PhoenixOdometryThread extends Thread { + private final Lock signalsLock = + new ReentrantLock(); // Prevents conflicts when registering signals + private BaseStatusSignal[] signals = new BaseStatusSignal[0]; + private final List> queues = new ArrayList<>(); + private final List> timestampQueues = new ArrayList<>(); + private boolean isCANFD = false; + + private static PhoenixOdometryThread instance = null; + + public static PhoenixOdometryThread getInstance() { + if (instance == null) { + instance = new PhoenixOdometryThread(); + } + return instance; + } + + private PhoenixOdometryThread() { + setName("PhoenixOdometryThread"); + setDaemon(true); + } + + @Override + public void start() { + if (timestampQueues.size() > 0) { + super.start(); + } + } + + public Queue registerSignal(ParentDevice device, StatusSignal signal) { + Queue queue = new ArrayBlockingQueue<>(20); + signalsLock.lock(); + Drive.odometryLock.lock(); + try { + isCANFD = CANBus.isNetworkFD(device.getNetwork()); + BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; + System.arraycopy(signals, 0, newSignals, 0, signals.length); + newSignals[signals.length] = signal; + signals = newSignals; + queues.add(queue); + } finally { + signalsLock.unlock(); + Drive.odometryLock.unlock(); + } + return queue; + } + + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>(20); + Drive.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } + + @Override + public void run() { + while (true) { + // Wait for updates from all signals + signalsLock.lock(); + try { + if (isCANFD) { + BaseStatusSignal.waitForAll(2.0 / Module.ODOMETRY_FREQUENCY, signals); + } else { + // "waitForAll" does not support blocking on multiple + // signals with a bus that is not CAN FD, regardless + // of Pro licensing. No reasoning for this behavior + // is provided by the documentation. + Thread.sleep((long) (1000.0 / Module.ODOMETRY_FREQUENCY)); + if (signals.length > 0) BaseStatusSignal.refreshAll(signals); + } + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + signalsLock.unlock(); + } + + // Save new data to queues + Drive.odometryLock.lock(); + try { + double timestamp = Logger.getRealTimestamp() / 1e6; + double totalLatency = 0.0; + for (BaseStatusSignal signal : signals) { + totalLatency += signal.getTimestamp().getLatency(); + } + if (signals.length > 0) { + timestamp -= totalLatency / signals.length; + } + for (int i = 0; i < signals.length; i++) { + queues.get(i).offer(signals[i].getValueAsDouble()); + } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } + } finally { + Drive.odometryLock.unlock(); + } + } + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java new file mode 100644 index 00000000..15db2664 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -0,0 +1,107 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import edu.wpi.first.wpilibj.Notifier; +import java.util.ArrayList; +import java.util.List; +import java.util.OptionalDouble; +import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + +/** + * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. + * + *

This version is intended for devices like the SparkMax that require polling rather than a + * blocking thread. A Notifier thread is used to gather samples with consistent timing. + */ +public class SparkMaxOdometryThread { + private List> signals = new ArrayList<>(); + private List> queues = new ArrayList<>(); + private List> timestampQueues = new ArrayList<>(); + + private final Notifier notifier; + private static SparkMaxOdometryThread instance = null; + + public static SparkMaxOdometryThread getInstance() { + if (instance == null) { + instance = new SparkMaxOdometryThread(); + } + return instance; + } + + private SparkMaxOdometryThread() { + notifier = new Notifier(this::periodic); + notifier.setName("SparkMaxOdometryThread"); + } + + public void start() { + if (timestampQueues.size() > 0) { + notifier.startPeriodic(1.0 / Module.ODOMETRY_FREQUENCY); + } + } + + public Queue registerSignal(Supplier signal) { + Queue queue = new ArrayBlockingQueue<>(20); + Drive.odometryLock.lock(); + try { + signals.add(signal); + queues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } + + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>(20); + Drive.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } + + private void periodic() { + Drive.odometryLock.lock(); + double timestamp = Logger.getRealTimestamp() / 1e6; + try { + double[] values = new double[signals.size()]; + boolean isValid = true; + for (int i = 0; i < signals.size(); i++) { + OptionalDouble value = signals.get(i).get(); + if (value.isPresent()) { + values[i] = value.getAsDouble(); + } else { + isValid = false; + break; + } + } + if (isValid) { + for (int i = 0; i < queues.size(); i++) { + queues.get(i).offer(values[i]); + } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } + } + } finally { + Drive.odometryLock.unlock(); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java new file mode 100644 index 00000000..342df9df --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java @@ -0,0 +1,110 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.flywheel; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class Flywheel extends SubsystemBase { + private final FlywheelIO io; + private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); + private final SimpleMotorFeedforward ffModel; + private final SysIdRoutine sysId; + + /** Creates a new Flywheel. */ + public Flywheel(FlywheelIO io) { + this.io = io; + + // Switch constants based on mode (the physics simulator is treated as a + // separate robot with different tuning) + switch (Constants.getMode()) { + case REAL: + case REPLAY: + ffModel = new SimpleMotorFeedforward(0.1, 0.05); + io.configurePID(1.0, 0.0, 0.0); + break; + case SIM: + ffModel = new SimpleMotorFeedforward(0.0, 0.03); + io.configurePID(0.5, 0.0, 0.0); + break; + default: + ffModel = new SimpleMotorFeedforward(0.0, 0.0); + break; + } + + // Configure SysId + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Flywheel/SysIdState", state.toString())), + new SysIdRoutine.Mechanism((voltage) -> runVolts(voltage.in(Volts)), null, this)); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Flywheel", inputs); + } + + /** Run open loop at the specified voltage. */ + public void runVolts(double volts) { + io.setVoltage(volts); + } + + /** Run closed loop at the specified velocity. */ + public void runVelocity(double velocityRPM) { + var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); + io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec)); + + // Log flywheel setpoint + Logger.recordOutput("Flywheel/SetpointRPM", velocityRPM); + } + + /** Stops the flywheel. */ + public void stop() { + io.stop(); + } + + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysId.quasistatic(direction); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysId.dynamic(direction); + } + + /** Returns the current velocity in RPM. */ + @AutoLogOutput + public double getVelocityRPM() { + return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec); + } + + /** Returns the current velocity in radians per second. */ + public double getCharacterizationVelocity() { + return inputs.velocityRadPerSec; + } +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java new file mode 100644 index 00000000..98f7624c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java @@ -0,0 +1,41 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.flywheel; + +import org.littletonrobotics.junction.AutoLog; + +public interface FlywheelIO { + @AutoLog + public static class FlywheelIOInputs { + public double positionRad = 0.0; + public double velocityRadPerSec = 0.0; + public double appliedVolts = 0.0; + public double[] currentAmps = new double[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(FlywheelIOInputs inputs) {} + + /** Run open loop at the specified voltage. */ + public default void setVoltage(double volts) {} + + /** Run closed loop at the specified velocity. */ + public default void setVelocity(double velocityRadPerSec, double ffVolts) {} + + /** Stop in open loop. */ + public default void stop() {} + + /** Set velocity PID constants. */ + public default void configurePID(double kP, double kI, double kD) {} +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java new file mode 100644 index 00000000..32ffa6f3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java @@ -0,0 +1,68 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.flywheel; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; + +public class FlywheelIOSim implements FlywheelIO { + private FlywheelSim sim = new FlywheelSim(DCMotor.getNEO(1), 1.5, 0.004); + private PIDController pid = new PIDController(0.0, 0.0, 0.0); + + private boolean closedLoop = false; + private double ffVolts = 0.0; + private double appliedVolts = 0.0; + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + if (closedLoop) { + appliedVolts = + MathUtil.clamp(pid.calculate(sim.getAngularVelocityRadPerSec()) + ffVolts, -12.0, 12.0); + sim.setInputVoltage(appliedVolts); + } + + sim.update(0.02); + + inputs.positionRad = 0.0; + inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec(); + inputs.appliedVolts = appliedVolts; + inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()}; + } + + @Override + public void setVoltage(double volts) { + closedLoop = false; + appliedVolts = volts; + sim.setInputVoltage(volts); + } + + @Override + public void setVelocity(double velocityRadPerSec, double ffVolts) { + closedLoop = true; + pid.setSetpoint(velocityRadPerSec); + this.ffVolts = ffVolts; + } + + @Override + public void stop() { + setVoltage(0.0); + } + + @Override + public void configurePID(double kP, double kI, double kD) { + pid.setPID(kP, kI, kD); + } +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java new file mode 100644 index 00000000..787f3693 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java @@ -0,0 +1,89 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.flywheel; + +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkPIDController.ArbFFUnits; +import edu.wpi.first.math.util.Units; + +/** + * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with + * "CANSparkFlex". + */ +public class FlywheelIOSparkMax implements FlywheelIO { + private static final double GEAR_RATIO = 1.5; + + private final CANSparkMax leader = new CANSparkMax(0, MotorType.kBrushless); + private final CANSparkMax follower = new CANSparkMax(1, MotorType.kBrushless); + private final RelativeEncoder encoder = leader.getEncoder(); + private final SparkPIDController pid = leader.getPIDController(); + + public FlywheelIOSparkMax() { + leader.restoreFactoryDefaults(); + follower.restoreFactoryDefaults(); + + leader.setCANTimeout(250); + follower.setCANTimeout(250); + + leader.setInverted(false); + follower.follow(leader, false); + + leader.enableVoltageCompensation(12.0); + leader.setSmartCurrentLimit(30); + + leader.burnFlash(); + follower.burnFlash(); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + inputs.positionRad = Units.rotationsToRadians(encoder.getPosition() / GEAR_RATIO); + inputs.velocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / GEAR_RATIO); + inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage(); + inputs.currentAmps = new double[] {leader.getOutputCurrent(), follower.getOutputCurrent()}; + } + + @Override + public void setVoltage(double volts) { + leader.setVoltage(volts); + } + + @Override + public void setVelocity(double velocityRadPerSec, double ffVolts) { + pid.setReference( + Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO, + ControlType.kVelocity, + 0, + ffVolts, + ArbFFUnits.kVoltage); + } + + @Override + public void stop() { + leader.stopMotor(); + } + + @Override + public void configurePID(double kP, double kI, double kD) { + pid.setP(kP, 0); + pid.setI(kI, 0); + pid.setD(kD, 0); + pid.setFF(0, 0); + } +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java new file mode 100644 index 00000000..3f535129 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java @@ -0,0 +1,98 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.flywheel; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.util.Units; + +public class FlywheelIOTalonFX implements FlywheelIO { + private static final double GEAR_RATIO = 1.5; + + private final TalonFX leader = new TalonFX(0); + private final TalonFX follower = new TalonFX(1); + + private final StatusSignal leaderPosition = leader.getPosition(); + private final StatusSignal leaderVelocity = leader.getVelocity(); + private final StatusSignal leaderAppliedVolts = leader.getMotorVoltage(); + private final StatusSignal leaderCurrent = leader.getSupplyCurrent(); + private final StatusSignal followerCurrent = follower.getSupplyCurrent(); + + public FlywheelIOTalonFX() { + var config = new TalonFXConfiguration(); + config.CurrentLimits.SupplyCurrentLimit = 30.0; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + leader.getConfigurator().apply(config); + follower.getConfigurator().apply(config); + follower.setControl(new Follower(leader.getDeviceID(), false)); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); + leader.optimizeBusUtilization(); + follower.optimizeBusUtilization(); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + BaseStatusSignal.refreshAll( + leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); + inputs.positionRad = Units.rotationsToRadians(leaderPosition.getValueAsDouble()) / GEAR_RATIO; + inputs.velocityRadPerSec = + Units.rotationsToRadians(leaderVelocity.getValueAsDouble()) / GEAR_RATIO; + inputs.appliedVolts = leaderAppliedVolts.getValueAsDouble(); + inputs.currentAmps = + new double[] {leaderCurrent.getValueAsDouble(), followerCurrent.getValueAsDouble()}; + } + + @Override + public void setVoltage(double volts) { + leader.setControl(new VoltageOut(volts)); + } + + @Override + public void setVelocity(double velocityRadPerSec, double ffVolts) { + leader.setControl( + new VelocityVoltage( + Units.radiansToRotations(velocityRadPerSec), + 0.0, + true, + ffVolts, + 0, + false, + false, + false)); + } + + @Override + public void stop() { + leader.stopMotor(); + } + + @Override + public void configurePID(double kP, double kI, double kD) { + var config = new Slot0Configs(); + config.kP = kP; + config.kI = kI; + config.kD = kD; + leader.getConfigurator().apply(config); + } +} diff --git a/src/main/java/frc/robot/util/Alert.java b/src/main/java/frc/robot/util/Alert.java index 28adb52a..5608c3b7 100644 --- a/src/main/java/frc/robot/util/Alert.java +++ b/src/main/java/frc/robot/util/Alert.java @@ -1,7 +1,7 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage -// Copyright (c) 2024 FRC 2486 -// http://github.com/Coconuts2486-FRC // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java index 7712de62..550d7e9c 100644 --- a/src/main/java/frc/robot/util/AllianceFlipUtil.java +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -1,7 +1,7 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage -// Copyright (c) 2024 FRC 2486 -// http://github.com/Coconuts2486-FRC // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java index 4df60695..3a59a7fd 100644 --- a/src/main/java/frc/robot/util/GeomUtil.java +++ b/src/main/java/frc/robot/util/GeomUtil.java @@ -1,7 +1,7 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage -// Copyright (c) 2024 FRC 2486 -// http://github.com/Coconuts2486-FRC // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java index 191b54f2..08b61727 100644 --- a/src/main/java/frc/robot/util/LocalADStarAK.java +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -1,3 +1,18 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright (c) 2024 Michael Jansen +// http://gist.github.com/mjansen4857 +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util; import com.pathplanner.lib.path.GoalEndState; @@ -15,6 +30,7 @@ import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.inputs.LoggableInputs; +// PathPlannerLib pathfinder compatible with AdvantageKit log replay // NOTE: This file is available at // https://gist.github.com/mjansen4857/a8024b55eb427184dbd10ae8923bd57d diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index d3ff834b..3ab4f3d9 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -1,7 +1,7 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage -// Copyright (c) 2024 FRC 2486 -// http://github.com/Coconuts2486-FRC // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License diff --git a/src/main/java/frc/robot/util/OverrideSwitches.java b/src/main/java/frc/robot/util/OverrideSwitches.java index ef38b7a3..05576bed 100644 --- a/src/main/java/frc/robot/util/OverrideSwitches.java +++ b/src/main/java/frc/robot/util/OverrideSwitches.java @@ -1,7 +1,7 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage -// Copyright (c) 2024 FRC 2486 -// http://github.com/Coconuts2486-FRC // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License @@ -21,17 +21,17 @@ /** Interface for physical override switches on operator console. */ public class OverrideSwitches { - private final GenericHID joystick; + private final GenericHID consoleSwitches; public OverrideSwitches(int port) { - joystick = new GenericHID(port); + consoleSwitches = new GenericHID(port); } /** Returns whether the controller is connected. */ public boolean isConnected() { - return joystick.isConnected() - && !DriverStation.getJoystickIsXbox(joystick.getPort()) - && joystick.getName().equals("Generic USB Joystick"); + return consoleSwitches.isConnected() + && !DriverStation.getJoystickIsXbox(consoleSwitches.getPort()) + && consoleSwitches.getName().equals("Generic USB Joystick"); } /** Gets the state of a driver-side switch (0-2 from left to right). */ @@ -40,7 +40,7 @@ public boolean getDriverSwitch(int index) { throw new RuntimeException( "Invalid driver override index " + Integer.toString(index) + ". Must be 0-2."); } - return joystick.getRawButton(index + 1); + return consoleSwitches.getRawButton(index + 1); } /** Gets the state of an operator-side switch (0-4 from left to right). */ @@ -49,14 +49,14 @@ public boolean getOperatorSwitch(int index) { throw new RuntimeException( "Invalid operator override index " + Integer.toString(index) + ". Must be 0-4."); } - return joystick.getRawButton(index + 8); + return consoleSwitches.getRawButton(index + 8); } /** Gets the state of the multi-directional switch. */ public MultiDirectionSwitchState getMultiDirectionSwitch() { - if (joystick.getRawButton(4)) { + if (consoleSwitches.getRawButton(4)) { return MultiDirectionSwitchState.LEFT; - } else if (joystick.getRawButton(5)) { + } else if (consoleSwitches.getRawButton(5)) { return MultiDirectionSwitchState.RIGHT; } else { return MultiDirectionSwitchState.NEUTRAL; diff --git a/src/main/java/frc/robot/util/VirtualSubsystem.java b/src/main/java/frc/robot/util/VirtualSubsystem.java index 55f2cdd5..2943eb06 100644 --- a/src/main/java/frc/robot/util/VirtualSubsystem.java +++ b/src/main/java/frc/robot/util/VirtualSubsystem.java @@ -1,7 +1,7 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage -// Copyright (c) 2024 FRC 2486 -// http://github.com/Coconuts2486-FRC // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License @@ -24,15 +24,18 @@ public abstract class VirtualSubsystem { private static List subsystems = new ArrayList<>(); + // Load all defined virtual subsystems into a list public VirtualSubsystem() { subsystems.add(this); } public static void periodicAll() { + // Call each virtual subsystem during robotPeriodic() for (VirtualSubsystem subsystem : subsystems) { subsystem.periodic(); } } + // Each virtual subsystem must implement its own periodic() method public abstract void periodic(); } diff --git a/src/main/java/frc/robot/util/swerve/ModuleLimits.java b/src/main/java/frc/robot/util/swerve/ModuleLimits.java index b08e4a8c..63282e2e 100644 --- a/src/main/java/frc/robot/util/swerve/ModuleLimits.java +++ b/src/main/java/frc/robot/util/swerve/ModuleLimits.java @@ -1,11 +1,17 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage -// Copyright (c) 2024 FRC 2486 -// http://github.com/Coconuts2486-FRC // -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util.swerve; From 3c15202b011f58c6f4533b6ec47454189cb3baba Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 25 Sep 2024 10:41:27 -0700 Subject: [PATCH 04/57] Add the YAGSL example project TODO: Still need to meld together the YAGSL commands and subsystems with those from the AdvantageKit to make something that works. modified: README.md modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/RobotContainer.java new file: src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java new file: src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java new file: src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java new file: src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java new file: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java new file: src/main/java/frc/robot/subsystems/swervedrive/Vision.java --- README.md | 12 +- src/main/java/frc/robot/Constants.java | 51 +- src/main/java/frc/robot/Robot.java | 33 +- src/main/java/frc/robot/RobotContainer.java | 98 ++- .../swervedrive/auto/AutoBalanceCommand.java | 95 +++ .../swervedrive/drivebase/AbsoluteDrive.java | 135 ++++ .../drivebase/AbsoluteDriveAdv.java | 167 +++++ .../drivebase/AbsoluteFieldDrive.java | 104 +++ .../swervedrive/SwerveSubsystem.java | 629 ++++++++++++++++++ .../robot/subsystems/swervedrive/Vision.java | 445 +++++++++++++ 10 files changed, 1759 insertions(+), 10 deletions(-) create mode 100644 src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java create mode 100644 src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java create mode 100644 src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java create mode 100644 src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/Vision.java diff --git a/README.md b/README.md index 2e720d93..5cd52d31 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,7 @@ # Az-RBSI Arizona's Reference Build and Software Implementation for FRC Robots (read: "A-Z-ribsy") + ## Purpose The purpose of Az-RBSI is to help Arizona FRC teams with: @@ -25,10 +26,11 @@ and currently maintained FIRST- and community-sponsored software libraries to provide a baseline robot functionality that combines robust reliability with effective logging for troubleshooting. + ## Library Dependencies -* [WPILib](https://docs.wpilib.org/en/stable/index.html) -* [AdvantageKit](https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/docs/WHAT-IS-ADVANTAGEKIT.md) -* [YAGSL](https://yagsl.gitbook.io/yagsl) -* [PathPlanner](https://pathplanner.dev/home.html) -* [PhotonVision](https://docs.photonvision.org/en/latest/) +* [WPILib](https://docs.wpilib.org/en/stable/index.html) -- FIRST basic libraries +* [AdvantageKit](https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/docs/WHAT-IS-ADVANTAGEKIT.md) -- Logging +* [YAGSL](https://yagsl.gitbook.io/yagsl) -- Swerve drive library +* [PathPlanner](https://pathplanner.dev/home.html) -- Autonomous path planning +* [PhotonVision](https://docs.photonvision.org/en/latest/) -- Robot vision / tracking diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ee6fb926..0649f357 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,4 +1,6 @@ -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright (c) 2021-2024 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -10,12 +12,20 @@ // 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. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. package frc.robot; +import com.pathplanner.lib.util.PIDConstants; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; +import swervelib.math.Matter; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -26,10 +36,45 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + + /* General Constants */ public static final double loopPeriodSecs = 0.02; private static RobotType robotType = RobotType.COMPBOT; public static final boolean tuningMode = false; + /* Physical Constants for Robot Operation */ + public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound + public static final Matter CHASSIS = + new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); + public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms sprk max velocity lag + public static final double MAX_SPEED = Units.feetToMeters(14.5); + // Maximum speed of the robot in meters per second, used to limit acceleration. + + /** Autonomous Action Constants */ + public static final class AutonConstants { + + public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0); + public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01); + } + + /** Drive Base Constants */ + public static final class DrivebaseConstants { + + // Hold time on motor brakes when disabled + public static final double WHEEL_LOCK_TIME = 10; // seconds + } + + /** Operator Constants */ + public static class OperatorConstants { + + // Joystick Deadband + public static final double LEFT_X_DEADBAND = 0.1; + public static final double LEFT_Y_DEADBAND = 0.1; + public static final double RIGHT_X_DEADBAND = 0.1; + public static final double TURN_CONSTANT = 6; + } + + /** Location to define multiple robots (e.g., COMPBOT, DEVBOT, SIMBOT, etc.) */ public static RobotType getRobot() { if (!disableHAL && RobotBase.isReal() && robotType == RobotType.SIMBOT) { new Alert("Invalid robot selected, using competition robot as default.", AlertType.ERROR) @@ -39,6 +84,7 @@ public static RobotType getRobot() { return robotType; } + /** Get the current mode for robot operation, based on robot type */ public static Mode getMode() { return switch (robotType) { case DEVBOT, COMPBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; @@ -46,6 +92,7 @@ public static Mode getMode() { }; } + /** Enumerate the robot operation modes */ public static enum Mode { /** Running on a real robot. */ REAL, @@ -57,12 +104,14 @@ public static enum Mode { REPLAY } + /** Enumerate the robot types (add additional bots here) */ public enum RobotType { SIMBOT, DEVBOT, COMPBOT } + /** Disable the Hardware Abstraction Layer, if needed */ public static boolean disableHAL = false; public static void disableHAL() { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0c1c9f51..679551dc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,9 +13,13 @@ package frc.robot; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.util.VirtualSubsystem; +import java.io.File; +import java.io.IOException; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LogTable; import org.littletonrobotics.junction.LoggedRobot; @@ -23,6 +27,7 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import swervelib.parser.SwerveParser; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -33,6 +38,7 @@ public class Robot extends LoggedRobot { private Command autonomousCommand; private RobotContainer robotContainer; + private Timer disabledTimer; /** * This function is run when the robot is first started up and should be used for any @@ -92,6 +98,10 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. robotContainer = new RobotContainer(); + + // Create a timer to disable motor brake a few seconds after disable. This will let the robot + // stop immediately when disabled, but then also let it be pushed more + disabledTimer = new Timer(); } /** This function is called periodically during all modes. */ @@ -110,15 +120,25 @@ public void robotPeriodic() { /** This function is called once when the robot is disabled. */ @Override - public void disabledInit() {} + public void disabledInit() { + robotContainer.setMotorBrake(true); + disabledTimer.reset(); + disabledTimer.start(); + } /** This function is called periodically when disabled. */ @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + if (disabledTimer.hasElapsed(Constants.DrivebaseConstants.WHEEL_LOCK_TIME)) { + robotContainer.setMotorBrake(false); + disabledTimer.stop(); + } + } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { + robotContainer.setMotorBrake(true); autonomousCommand = robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) @@ -141,6 +161,8 @@ public void teleopInit() { if (autonomousCommand != null) { autonomousCommand.cancel(); } + robotContainer.setDriveMode(); + robotContainer.setMotorBrake(true); } /** This function is called periodically during operator control. */ @Override @@ -151,6 +173,13 @@ public void teleopPeriodic() {} public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); + + // Try setting up swerve + try { + new SwerveParser(new File(Filesystem.getDeployDirectory(), "swerve")); + } catch (IOException e) { + throw new RuntimeException(e); + } } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d09..81abfe99 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,17 +4,111 @@ package frc.robot; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.OperatorConstants; +import frc.robot.commands.swervedrive.drivebase.AbsoluteDriveAdv; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import java.io.File; public class RobotContainer { + + // Replace with CommandPS4Controller or CommandJoystick if needed + final CommandXboxController driverXbox = new CommandXboxController(0); + // The robot's subsystems and commands are defined here... + private final SwerveSubsystem drivebase = + new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo")); + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + + // Configure the trigger bindings configureBindings(); + + // Applies deadbands and inverts controls because joysticks + // are back-right positive while robot + // controls are front-left positive + // left stick controls translation + // right stick controls the rotational velocity + // buttons are quick rotation positions to different ways to face + // WARNING: default buttons are on the same buttons as the ones defined in configureBindings + AbsoluteDriveAdv closedAbsoluteDriveAdv = + new AbsoluteDriveAdv( + drivebase, + () -> -MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), + () -> -MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), + () -> + -MathUtil.applyDeadband(driverXbox.getRightX(), OperatorConstants.RIGHT_X_DEADBAND), + driverXbox.getHID()::getYButtonPressed, + driverXbox.getHID()::getAButtonPressed, + driverXbox.getHID()::getXButtonPressed, + driverXbox.getHID()::getBButtonPressed); + + // Applies deadbands and inverts controls because joysticks + // are back-right positive while robot + // controls are front-left positive + // left stick controls translation + // right stick controls the desired angle NOT angular rotation + Command driveFieldOrientedDirectAngle = + drivebase.driveCommand( + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), + () -> driverXbox.getRightX(), + () -> driverXbox.getRightY()); + + // Applies deadbands and inverts controls because joysticks + // are back-right positive while robot + // controls are front-left positive + // left stick controls translation + // right stick controls the angular velocity of the robot + Command driveFieldOrientedAnglularVelocity = + drivebase.driveCommand( + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), + () -> driverXbox.getRightX() * 0.5); + + Command driveFieldOrientedDirectAngleSim = + drivebase.simDriveCommand( + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), + () -> driverXbox.getRawAxis(2)); + + drivebase.setDefaultCommand( + !RobotBase.isSimulation() + ? driveFieldOrientedDirectAngle + : driveFieldOrientedDirectAngleSim); } + /** + * Use this method to define your trigger->command mappings. Triggers can be created via the + * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary + * predicate, or via the named factories in {@link + * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link + * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller + * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight + * joysticks}. + */ private void configureBindings() {} + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + // An example command will be run in autonomous + return drivebase.getAutonomousCommand("New Auto"); + } + + public void setDriveMode() { + // drivebase.setDefaultCommand(); + } + + public void setMotorBrake(boolean brake) { + drivebase.setMotorBrake(brake); } } diff --git a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java b/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java new file mode 100644 index 00000000..27181671 --- /dev/null +++ b/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java @@ -0,0 +1,95 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. +// +// NOTE: This module from the YAGSL Example Project + +package frc.robot.commands.swervedrive.auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; + +/** + * Auto Balance command using a simple PID controller. Created by Team 3512 ... + */ +public class AutoBalanceCommand extends Command { + + private final SwerveSubsystem swerveSubsystem; + private final PIDController controller; + + public AutoBalanceCommand(SwerveSubsystem swerveSubsystem) { + this.swerveSubsystem = swerveSubsystem; + controller = new PIDController(1.0, 0.0, 0.0); + controller.setTolerance(1); + controller.setSetpoint(0.0); + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.swerveSubsystem); + } + + /** The initial subroutine of a command. Called once when the command is initially scheduled. */ + @Override + public void initialize() {} + + /** + * The main body of a command. Called repeatedly while the command is scheduled. (That is, it is + * called repeatedly until {@link #isFinished()}) returns true.) + */ + @Override + public void execute() { + SmartDashboard.putBoolean("At Tolerance", controller.atSetpoint()); + + double translationVal = + MathUtil.clamp( + controller.calculate(swerveSubsystem.getPitch().getDegrees(), 0.0), -0.5, 0.5); + swerveSubsystem.drive(new Translation2d(translationVal, 0.0), 0.0, true); + } + + /** + * Returns whether this command has finished. Once a command finishes -- indicated by this method + * returning true -- the scheduler will call its {@link #end(boolean)} method. + * + *

Returning false will result in the command never ending automatically. It may still be + * cancelled manually or interrupted by another command. Hard coding this command to always return + * true will result in the command executing once and finishing immediately. It is recommended to + * use * {@link edu.wpi.first.wpilibj2.command.InstantCommand InstantCommand} for such an + * operation. + * + * @return whether this command has finished. + */ + @Override + public boolean isFinished() { + return controller.atSetpoint(); + } + + /** + * The action to take when the command ends. Called when either the command finishes normally -- + * that is it is called when {@link #isFinished()} returns true -- or when it is + * interrupted/canceled. This is where you may want to wrap up loose ends, like shutting off a + * motor that was being used in the command. + * + * @param interrupted whether the command was interrupted/canceled + */ + @Override + public void end(boolean interrupted) { + swerveSubsystem.lock(); + } +} diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java new file mode 100644 index 00000000..4cb7018b --- /dev/null +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java @@ -0,0 +1,135 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. +// +// NOTE: This module from the YAGSL Example Project + +package frc.robot.commands.swervedrive.drivebase; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import java.util.List; +import java.util.function.DoubleSupplier; +import swervelib.SwerveController; +import swervelib.math.SwerveMath; + +/** An example command that uses an example subsystem. */ +public class AbsoluteDrive extends Command { + + private final SwerveSubsystem swerve; + private final DoubleSupplier vX, vY; + private final DoubleSupplier headingHorizontal, headingVertical; + private boolean initRotation = false; + + /** + * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, + * where x is torwards/away from alliance wall and y is left/right. headingHorzontal and + * headingVertical are the Cartesian coordinates from which the robot's angle will be derived— + * they will be converted to a polar angle, which the robot will rotate to. + * + * @param swerve The swerve drivebase subsystem. + * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. + * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when + * looking through the driver station glass. + * @param headingHorizontal DoubleSupplier that supplies the horizontal component of the robot's + * heading angle. In the robot coordinate system, this is along the same axis as vY. Should + * range from -1 to 1 with no deadband. Positive is towards the left wall when looking through + * the driver station glass. + * @param headingVertical DoubleSupplier that supplies the vertical component of the robot's + * heading angle. In the robot coordinate system, this is along the same axis as vX. Should + * range from -1 to 1 with no deadband. Positive is away from the alliance wall. + */ + public AbsoluteDrive( + SwerveSubsystem swerve, + DoubleSupplier vX, + DoubleSupplier vY, + DoubleSupplier headingHorizontal, + DoubleSupplier headingVertical) { + this.swerve = swerve; + this.vX = vX; + this.vY = vY; + this.headingHorizontal = headingHorizontal; + this.headingVertical = headingVertical; + + addRequirements(swerve); + } + + @Override + public void initialize() { + initRotation = true; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + // Get the desired chassis speeds based on a 2 joystick module. + ChassisSpeeds desiredSpeeds = + swerve.getTargetSpeeds( + vX.getAsDouble(), + vY.getAsDouble(), + headingHorizontal.getAsDouble(), + headingVertical.getAsDouble()); + + // Prevent Movement After Auto + if (initRotation) { + if (headingHorizontal.getAsDouble() == 0 && headingVertical.getAsDouble() == 0) { + // Get the curretHeading + Rotation2d firstLoopHeading = swerve.getHeading(); + + // Set the Current Heading to the desired Heading + desiredSpeeds = + swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos()); + } + // Dont Init Rotation Again + initRotation = false; + } + + // Limit velocity to prevent tippy + Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); + translation = + SwerveMath.limitVelocity( + translation, + swerve.getFieldVelocity(), + swerve.getPose(), + Constants.LOOP_TIME, + Constants.ROBOT_MASS, + List.of(Constants.CHASSIS), + swerve.getSwerveDriveConfiguration()); + SmartDashboard.putNumber("LimitedTranslation", translation.getX()); + SmartDashboard.putString("Translation", translation.toString()); + + // Make the robot move + swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java new file mode 100644 index 00000000..33aff281 --- /dev/null +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java @@ -0,0 +1,167 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. +// +// NOTE: This module from the YAGSL Example Project + +package frc.robot.commands.swervedrive.drivebase; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import java.util.List; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import swervelib.SwerveController; +import swervelib.math.SwerveMath; + +/** A more advanced Swerve Control System that has 4 buttons for which direction to face */ +public class AbsoluteDriveAdv extends Command { + + private final SwerveSubsystem swerve; + private final DoubleSupplier vX, vY; + private final DoubleSupplier headingAdjust; + private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; + private boolean resetHeading = false; + + /** + * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, + * where x is torwards/away from alliance wall and y is left/right. Heading Adjust changes the + * current heading after being multipied by a constant. The look booleans are shortcuts to get the + * robot to face a certian direction. Based off of ideas in + * https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172 + * + * @param swerve The swerve drivebase subsystem. + * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. + * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when + * looking through the driver station glass. + * @param headingAdjust DoubleSupplier that supplies the component of the robot's heading angle + * that should be adjusted. Should range from -1 to 1 with deadband already accounted for. + * @param lookAway Face the robot towards the opposing alliance's wall in the same direction the + * driver is facing + * @param lookTowards Face the robot towards the driver + * @param lookLeft Face the robot left + * @param lookRight Face the robot right + */ + public AbsoluteDriveAdv( + SwerveSubsystem swerve, + DoubleSupplier vX, + DoubleSupplier vY, + DoubleSupplier headingAdjust, + BooleanSupplier lookAway, + BooleanSupplier lookTowards, + BooleanSupplier lookLeft, + BooleanSupplier lookRight) { + this.swerve = swerve; + this.vX = vX; + this.vY = vY; + this.headingAdjust = headingAdjust; + this.lookAway = lookAway; + this.lookTowards = lookTowards; + this.lookLeft = lookLeft; + this.lookRight = lookRight; + + addRequirements(swerve); + } + + @Override + public void initialize() { + resetHeading = true; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double headingX = 0; + double headingY = 0; + + // These are written to allow combinations for 45 angles + // Face Away from Drivers + if (lookAway.getAsBoolean()) { + headingY = -1; + } + // Face Right + if (lookRight.getAsBoolean()) { + headingX = 1; + } + // Face Left + if (lookLeft.getAsBoolean()) { + headingX = -1; + } + // Face Towards the Drivers + if (lookTowards.getAsBoolean()) { + headingY = 1; + } + + // Prevent Movement After Auto + if (resetHeading) { + if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) == 0) { + // Get the curret Heading + Rotation2d currentHeading = swerve.getHeading(); + + // Set the Current Heading to the desired Heading + headingX = currentHeading.getSin(); + headingY = currentHeading.getCos(); + } + // Dont reset Heading Again + resetHeading = false; + } + + ChassisSpeeds desiredSpeeds = + swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY); + + // Limit velocity to prevent tippy + Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); + translation = + SwerveMath.limitVelocity( + translation, + swerve.getFieldVelocity(), + swerve.getPose(), + Constants.LOOP_TIME, + Constants.ROBOT_MASS, + List.of(Constants.CHASSIS), + swerve.getSwerveDriveConfiguration()); + SmartDashboard.putNumber("LimitedTranslation", translation.getX()); + SmartDashboard.putString("Translation", translation.toString()); + + // Make the robot move + if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { + resetHeading = true; + swerve.drive( + translation, + (Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), + true); + } else { + swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java new file mode 100644 index 00000000..4b6d16fc --- /dev/null +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java @@ -0,0 +1,104 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. +// +// NOTE: This module from the YAGSL Example Project + +package frc.robot.commands.swervedrive.drivebase; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import java.util.List; +import java.util.function.DoubleSupplier; +import swervelib.SwerveController; +import swervelib.math.SwerveMath; + +/** An example command that uses an example subsystem. */ +public class AbsoluteFieldDrive extends Command { + + private final SwerveSubsystem swerve; + private final DoubleSupplier vX, vY, heading; + + /** + * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, + * where x is torwards/away from alliance wall and y is left/right. headingHorzontal and + * headingVertical are the Cartesian coordinates from which the robot's angle will be derived— + * they will be converted to a polar angle, which the robot will rotate to. + * + * @param swerve The swerve drivebase subsystem. + * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. + * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when + * looking through the driver station glass. + * @param heading DoubleSupplier that supplies the robot's heading angle. + */ + public AbsoluteFieldDrive( + SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier heading) { + this.swerve = swerve; + this.vX = vX; + this.vY = vY; + this.heading = heading; + + addRequirements(swerve); + } + + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + // Get the desired chassis speeds based on a 2 joystick module. + + ChassisSpeeds desiredSpeeds = + swerve.getTargetSpeeds( + vX.getAsDouble(), vY.getAsDouble(), new Rotation2d(heading.getAsDouble() * Math.PI)); + + // Limit velocity to prevent tippy + Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); + translation = + SwerveMath.limitVelocity( + translation, + swerve.getFieldVelocity(), + swerve.getPose(), + Constants.LOOP_TIME, + Constants.ROBOT_MASS, + List.of(Constants.CHASSIS), + swerve.getSwerveDriveConfiguration()); + SmartDashboard.putNumber("LimitedTranslation", translation.getX()); + SmartDashboard.putString("Translation", translation.toString()); + + // Make the robot move + swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java new file mode 100644 index 00000000..37f5da40 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -0,0 +1,629 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. +// +// NOTE: This module from the YAGSL Example Project + +package frc.robot.subsystems.swervedrive; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +import frc.robot.Constants; +import frc.robot.Constants.AutonConstants; +import java.io.File; +import java.util.function.DoubleSupplier; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; +import swervelib.SwerveController; +import swervelib.SwerveDrive; +import swervelib.SwerveDriveTest; +import swervelib.math.SwerveMath; +import swervelib.parser.SwerveControllerConfiguration; +import swervelib.parser.SwerveDriveConfiguration; +import swervelib.parser.SwerveParser; +import swervelib.telemetry.SwerveDriveTelemetry; +import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; + +/** SwerveSubsystem module */ +public class SwerveSubsystem extends SubsystemBase { + + /** PhotonVision class to keep an accurate odometry. */ + private Vision vision; + /** Swerve drive object. */ + private final SwerveDrive swerveDrive; + /** AprilTag field layout. */ + private final AprilTagFieldLayout aprilTagFieldLayout = + AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + /** Enable vision odometry updates while driving. */ + private final boolean visionDriveTest = false; + + /** + * Initialize {@link SwerveDrive} with the directory provided. + * + * @param directory Directory of swerve drive config files. + */ + public SwerveSubsystem(File directory) { + // Angle conversion factor is 360 / (GEAR RATIO * ENCODER RESOLUTION) + // In this case the gear ratio is 12.8 motor revolutions per wheel rotation. + // The encoder resolution per motor revolution is 1 per motor revolution. + double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(12.8); + // Motor conversion factor is (PI * WHEEL DIAMETER IN METERS) / (GEAR RATIO * ENCODER + // RESOLUTION). + // In this case the wheel diameter is 4 inches, which must be converted to meters to get + // meters/second. + // The gear ratio is 6.75 motor revolutions per wheel rotation. + // The encoder resolution per motor revolution is 1 per motor revolution. + double driveConversionFactor = + SwerveMath.calculateMetersPerRotation(Units.inchesToMeters(4), 6.75); + System.out.println("\"conversionFactors\": {"); + System.out.println("\t\"angle\": {\"factor\": " + angleConversionFactor + " },"); + System.out.println("\t\"drive\": {\"factor\": " + driveConversionFactor + " }"); + System.out.println("}"); + + // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being + // created. + SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; + try { + swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED); + // Alternative method if you don't want to supply the conversion factor via JSON files. + // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, + // angleConversionFactor, driveConversionFactor); + } catch (Exception e) { + throw new RuntimeException(e); + } + swerveDrive.setHeadingCorrection( + false); // Heading correction should only be used while controlling the robot via angle. + swerveDrive.setCosineCompensator( + false); // !SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for + // simulations since it causes discrepancies not seen in real life. + if (visionDriveTest) { + setupPhotonVision(); + // Stop the odometry thread if we are using vision that way we can synchronize updates better. + swerveDrive.stopOdometryThread(); + } + setupPathPlanner(); + } + + /** + * Construct the swerve drive. + * + * @param driveCfg SwerveDriveConfiguration for the swerve. + * @param controllerCfg Swerve Controller. + */ + public SwerveSubsystem( + SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { + swerveDrive = new SwerveDrive(driveCfg, controllerCfg, Constants.MAX_SPEED); + } + + /** Setup the photon vision class. */ + public void setupPhotonVision() { + vision = new Vision(swerveDrive::getPose, swerveDrive.field); + } + + @Override + public void periodic() { + // When vision is enabled we must manually update odometry in SwerveDrive + if (visionDriveTest) { + swerveDrive.updateOdometry(); + vision.updatePoseEstimation(swerveDrive); + } + } + + @Override + public void simulationPeriodic() {} + + /** Setup AutoBuilder for PathPlanner. */ + public void setupPathPlanner() { + AutoBuilder.configureHolonomic( + this::getPose, // Robot pose supplier + this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting + // pose) + this::getRobotVelocity, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::setChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE + // ChassisSpeeds + new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in + // your Constants class + AutonConstants.TRANSLATION_PID, + // Translation PID constants + AutonConstants.ANGLE_PID, + // Rotation PID constants + 4.5, + // Max module speed, in m/s + swerveDrive.swerveDriveConfiguration.getDriveBaseRadiusMeters(), + // Drive base radius in meters. Distance from robot center to furthest module. + new ReplanningConfig() + // Default path replanning config. See the API for the options here + ), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + var alliance = DriverStation.getAlliance(); + return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; + }, + this // Reference to this subsystem to set requirements + ); + } + + /** + * Get the distance to the speaker. + * + * @return Distance to speaker in meters. + */ + public double getDistanceToSpeaker() { + int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; + // Taken from PhotonUtils.getDistanceToPose + Pose3d speakerAprilTagPose = aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); + return getPose().getTranslation().getDistance(speakerAprilTagPose.toPose2d().getTranslation()); + } + + /** + * Get the yaw to aim at the speaker. + * + * @return {@link Rotation2d} of which you need to achieve. + */ + public Rotation2d getSpeakerYaw() { + int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; + // Taken from PhotonUtils.getYawToPose() + Pose3d speakerAprilTagPose = aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); + Translation2d relativeTrl = + speakerAprilTagPose.toPose2d().relativeTo(getPose()).getTranslation(); + return new Rotation2d(relativeTrl.getX(), relativeTrl.getY()) + .plus(swerveDrive.getOdometryHeading()); + } + + /** + * Aim the robot at the speaker. + * + * @param tolerance Tolerance in degrees. + * @return Command to turn the robot to the speaker. + */ + public Command aimAtSpeaker(double tolerance) { + SwerveController controller = swerveDrive.getSwerveController(); + return run(() -> { + drive( + ChassisSpeeds.fromFieldRelativeSpeeds( + 0, + 0, + controller.headingCalculate( + getHeading().getRadians(), getSpeakerYaw().getRadians()), + getHeading())); + }) + .until(() -> getSpeakerYaw().minus(getHeading()).getDegrees() < tolerance); + } + + /** + * Aim the robot at the target returned by PhotonVision. + * + * @param camera {@link PhotonCamera} to communicate with. + * @return A {@link Command} which will run the alignment. + */ + public Command aimAtTarget(PhotonCamera camera) { + + return run( + () -> { + PhotonPipelineResult result = camera.getLatestResult(); + if (result.hasTargets()) { + drive( + getTargetSpeeds( + 0, + 0, + Rotation2d.fromDegrees( + result + .getBestTarget() + .getYaw()))); // Not sure if this will work, more math may be required. + } + }); + } + + /** + * Get the path follower with events. + * + * @param pathName PathPlanner path name. + * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. + */ + public Command getAutonomousCommand(String pathName) { + // Create a path following command using AutoBuilder. This will also trigger event markers. + return new PathPlannerAuto(pathName); + } + + /** + * Use PathPlanner Path finding to go to a point on the field. + * + * @param pose Target {@link Pose2d} to go to. + * @return PathFinding command + */ + public Command driveToPose(Pose2d pose) { + // Create the constraints to use while pathfinding + PathConstraints constraints = + new PathConstraints( + swerveDrive.getMaximumVelocity(), + 4.0, + swerveDrive.getMaximumAngularVelocity(), + Units.degreesToRadians(720)); + + // Since AutoBuilder is configured, we can use it to build pathfinding commands + return AutoBuilder.pathfindToPose( + pose, + constraints, + 0.0, // Goal end velocity in meters/sec + 0.0 // Rotation delay distance in meters. This is how far the robot should travel before + // attempting to rotate. + ); + } + + /** + * Command to drive the robot using translative values and heading as a setpoint. + * + * @param translationX Translation in the X direction. Cubed for smoother controls. + * @param translationY Translation in the Y direction. Cubed for smoother controls. + * @param headingX Heading X to calculate angle of the joystick. + * @param headingY Heading Y to calculate angle of the joystick. + * @return Drive command. + */ + public Command driveCommand( + DoubleSupplier translationX, + DoubleSupplier translationY, + DoubleSupplier headingX, + DoubleSupplier headingY) { + // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for + // this kind of control. + return run( + () -> { + Translation2d scaledInputs = + SwerveMath.scaleTranslation( + new Translation2d(translationX.getAsDouble(), translationY.getAsDouble()), 0.8); + + // Make the robot move + driveFieldOriented( + swerveDrive.swerveController.getTargetSpeeds( + scaledInputs.getX(), + scaledInputs.getY(), + headingX.getAsDouble(), + headingY.getAsDouble(), + swerveDrive.getOdometryHeading().getRadians(), + swerveDrive.getMaximumVelocity())); + }); + } + + /** + * Command to drive the robot using translative values and heading as a setpoint. + * + * @param translationX Translation in the X direction. + * @param translationY Translation in the Y direction. + * @param rotation Rotation as a value between [-1, 1] converted to radians. + * @return Drive command. + */ + public Command simDriveCommand( + DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier rotation) { + // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for + // this kind of control. + return run( + () -> { + // Make the robot move + driveFieldOriented( + swerveDrive.swerveController.getTargetSpeeds( + translationX.getAsDouble(), + translationY.getAsDouble(), + rotation.getAsDouble() * Math.PI, + swerveDrive.getOdometryHeading().getRadians(), + swerveDrive.getMaximumVelocity())); + }); + } + + /** + * Command to characterize the robot drive motors using SysId + * + * @return SysId Drive Command + */ + public Command sysIdDriveMotorCommand() { + return SwerveDriveTest.generateSysIdCommand( + SwerveDriveTest.setDriveSysIdRoutine(new Config(), this, swerveDrive, 12), 3.0, 5.0, 3.0); + } + + /** + * Command to characterize the robot angle motors using SysId + * + * @return SysId Angle Command + */ + public Command sysIdAngleMotorCommand() { + return SwerveDriveTest.generateSysIdCommand( + SwerveDriveTest.setAngleSysIdRoutine(new Config(), this, swerveDrive), 3.0, 5.0, 3.0); + } + + /** + * Command to drive the robot using translative values and heading as angular velocity. + * + * @param translationX Translation in the X direction. Cubed for smoother controls. + * @param translationY Translation in the Y direction. Cubed for smoother controls. + * @param angularRotationX Angular velocity of the robot to set. Cubed for smoother controls. + * @return Drive command. + */ + public Command driveCommand( + DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) { + return run( + () -> { + // Make the robot move + swerveDrive.drive( + SwerveMath.scaleTranslation( + new Translation2d( + translationX.getAsDouble() * swerveDrive.getMaximumVelocity(), + translationY.getAsDouble() * swerveDrive.getMaximumVelocity()), + 0.8), + Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumAngularVelocity(), + true, + false); + }); + } + + /** + * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation + * rate, and calculates and commands module states accordingly. Can use either open-loop or + * closed-loop velocity control for the wheel velocities. Also has field- and robot-relative + * modes, which affect how the translation vector is used. + * + * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in + * meters per second. In robot-relative mode, positive x is torwards the bow (front) and + * positive y is torwards port (left). In field-relative mode, positive x is away from the + * alliance wall (field North) and positive y is torwards the left wall when looking through + * the driver station glass (field West). + * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by + * field/robot relativity. + * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. + */ + public void drive(Translation2d translation, double rotation, boolean fieldRelative) { + swerveDrive.drive( + translation, + rotation, + fieldRelative, + false); // Open loop is disabled since it shouldn't be used most of the time. + } + + /** + * Drive the robot given a chassis field oriented velocity. + * + * @param velocity Velocity according to the field. + */ + public void driveFieldOriented(ChassisSpeeds velocity) { + swerveDrive.driveFieldOriented(velocity); + } + + /** + * Drive according to the chassis robot oriented velocity. + * + * @param velocity Robot oriented {@link ChassisSpeeds} + */ + public void drive(ChassisSpeeds velocity) { + swerveDrive.drive(velocity); + } + + /** + * Get the swerve drive kinematics object. + * + * @return {@link SwerveDriveKinematics} of the swerve drive. + */ + public SwerveDriveKinematics getKinematics() { + return swerveDrive.kinematics; + } + + /** + * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when + * calling this method. However, if either gyro angle or module position is reset, this must be + * called in order for odometry to keep working. + * + * @param initialHolonomicPose The pose to set the odometry to + */ + public void resetOdometry(Pose2d initialHolonomicPose) { + swerveDrive.resetOdometry(initialHolonomicPose); + } + + /** + * Gets the current pose (position and rotation) of the robot, as reported by odometry. + * + * @return The robot's pose + */ + public Pose2d getPose() { + return swerveDrive.getPose(); + } + + /** + * Set chassis speeds with closed-loop velocity control. + * + * @param chassisSpeeds Chassis Speeds to set. + */ + public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { + swerveDrive.setChassisSpeeds(chassisSpeeds); + } + + /** + * Post the trajectory to the field. + * + * @param trajectory The trajectory to post. + */ + public void postTrajectory(Trajectory trajectory) { + swerveDrive.postTrajectory(trajectory); + } + + /** + * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. + */ + public void zeroGyro() { + swerveDrive.zeroGyro(); + } + + /** + * Checks if the alliance is red, defaults to false if alliance isn't available. + * + * @return true if the red alliance, false if blue. Defaults to false if none is available. + */ + private boolean isRedAlliance() { + var alliance = DriverStation.getAlliance(); + return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; + } + + /** + * This will zero (calibrate) the robot to assume the current position is facing forward + * + *

If red alliance rotate the robot 180 after the drviebase zero command + */ + public void zeroGyroWithAlliance() { + if (isRedAlliance()) { + zeroGyro(); + // Set the pose 180 degrees + resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180))); + } else { + zeroGyro(); + } + } + + /** + * Sets the drive motors to brake/coast mode. + * + * @param brake True to set motors to brake mode, false for coast. + */ + public void setMotorBrake(boolean brake) { + swerveDrive.setMotorIdleMode(brake); + } + + /** + * Gets the current yaw angle of the robot, as reported by the swerve pose estimator in the + * underlying drivebase. Note, this is not the raw gyro reading, this may be corrected from calls + * to resetOdometry(). + * + * @return The yaw angle + */ + public Rotation2d getHeading() { + return getPose().getRotation(); + } + + /** + * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which + * direction. The other for the angle of the robot. + * + * @param xInput X joystick input for the robot to move in the X direction. + * @param yInput Y joystick input for the robot to move in the Y direction. + * @param headingX X joystick which controls the angle of the robot. + * @param headingY Y joystick which controls the angle of the robot. + * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. + */ + public ChassisSpeeds getTargetSpeeds( + double xInput, double yInput, double headingX, double headingY) { + Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); + return swerveDrive.swerveController.getTargetSpeeds( + scaledInputs.getX(), + scaledInputs.getY(), + headingX, + headingY, + getHeading().getRadians(), + Constants.MAX_SPEED); + } + + /** + * Get the chassis speeds based on controller input of 1 joystick and one angle. Control the robot + * at an offset of 90deg. + * + * @param xInput X joystick input for the robot to move in the X direction. + * @param yInput Y joystick input for the robot to move in the Y direction. + * @param angle The angle in as a {@link Rotation2d}. + * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. + */ + public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) { + Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); + + return swerveDrive.swerveController.getTargetSpeeds( + scaledInputs.getX(), + scaledInputs.getY(), + angle.getRadians(), + getHeading().getRadians(), + Constants.MAX_SPEED); + } + + /** + * Gets the current field-relative velocity (x, y and omega) of the robot + * + * @return A ChassisSpeeds object of the current field-relative velocity + */ + public ChassisSpeeds getFieldVelocity() { + return swerveDrive.getFieldVelocity(); + } + + /** + * Gets the current velocity (x, y and omega) of the robot + * + * @return A {@link ChassisSpeeds} object of the current velocity + */ + public ChassisSpeeds getRobotVelocity() { + return swerveDrive.getRobotVelocity(); + } + + /** + * Get the {@link SwerveController} in the swerve drive. + * + * @return {@link SwerveController} from the {@link SwerveDrive}. + */ + public SwerveController getSwerveController() { + return swerveDrive.swerveController; + } + + /** + * Get the {@link SwerveDriveConfiguration} object. + * + * @return The {@link SwerveDriveConfiguration} fpr the current drive. + */ + public SwerveDriveConfiguration getSwerveDriveConfiguration() { + return swerveDrive.swerveDriveConfiguration; + } + + /** Lock the swerve drive to prevent it from moving. */ + public void lock() { + swerveDrive.lockPose(); + } + + /** + * Gets the current pitch angle of the robot, as reported by the imu. + * + * @return The heading as a {@link Rotation2d} angle + */ + public Rotation2d getPitch() { + return swerveDrive.getPitch(); + } + + /** Add a fake vision reading for testing purposes. */ + public void addFakeVisionReading() { + swerveDrive.addVisionMeasurement( + new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp()); + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java new file mode 100644 index 00000000..41a881f5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -0,0 +1,445 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. +// +// NOTE: This module from the YAGSL Example Project + +package frc.robot.subsystems.swervedrive; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import frc.robot.Robot; +import java.awt.Desktop; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import java.util.function.Supplier; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.PhotonUtils; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import swervelib.SwerveDrive; +import swervelib.telemetry.Alert; +import swervelib.telemetry.Alert.AlertType; +import swervelib.telemetry.SwerveDriveTelemetry; + +/** + * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from + * https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads + */ +public class Vision { + + /** April Tag Field Layout of the year. */ + public static final AprilTagFieldLayout fieldLayout = + AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo); + /** Photon Vision Simulation */ + public VisionSystemSim visionSim; + /** Count of times that the odom thinks we're more than 10meters away from the april tag. */ + private double longDistangePoseEstimationCount = 0; + /** Current pose from the pose estimator using wheel odometry. */ + private Supplier currentPose; + /** Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}. */ + private final double maximumAmbiguity = 0.25; + /** Field from {@link swervelib.SwerveDrive#field} */ + private Field2d field2d; + + /** + * Constructor for the Vision class. + * + * @param currentPose Current pose supplier, should reference {@link SwerveDrive#getPose()} + * @param field Current field, should be {@link SwerveDrive#field} + */ + public Vision(Supplier currentPose, Field2d field) { + this.currentPose = currentPose; + this.field2d = field; + + if (Robot.isSimulation()) { + visionSim = new VisionSystemSim("Vision"); + visionSim.addAprilTags(fieldLayout); + + for (Cameras c : Cameras.values()) { + c.addToVisionSim(visionSim); + } + + openSimCameraViews(); + } + } + + /** + * Calculates a target pose relative to an AprilTag on the field. + * + * @param aprilTag The ID of the AprilTag. + * @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the + * robot to position itself correctly. + * @return The target pose of the AprilTag. + */ + public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) { + Optional aprilTagPose3d = fieldLayout.getTagPose(aprilTag); + if (aprilTagPose3d.isPresent()) { + return aprilTagPose3d.get().toPose2d().transformBy(robotOffset); + } else { + throw new RuntimeException( + "Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString()); + } + } + + /** + * Update the pose estimation inside of {@link SwerveDrive} with all of the given poses. + * + * @param swerveDrive {@link SwerveDrive} instance. + */ + public void updatePoseEstimation(SwerveDrive swerveDrive) { + if (SwerveDriveTelemetry.isSimulation) { + visionSim.update(swerveDrive.getPose()); + } + for (Cameras camera : Cameras.values()) { + Optional poseEst = getEstimatedGlobalPose(camera); + if (poseEst.isPresent()) { + var pose = poseEst.get(); + swerveDrive.addVisionMeasurement( + pose.estimatedPose.toPose2d(), pose.timestampSeconds, getEstimationStdDevs(camera)); + } + } + } + + /** + * Generates the estimated robot pose. Returns empty if: + * + *

    + *
  • No Pose Estimates could be generated + *
  • The generated pose estimate was considered not accurate + *
+ * + * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to + * create the estimate + */ + public Optional getEstimatedGlobalPose(Cameras camera) { + Optional poseEst = filterPose(camera.poseEstimator.update()); + // Uncomment to enable outputting of vision targets in sim. + /* + poseEst.ifPresent(estimatedRobotPose -> field2d.getObject(camera + " est pose") + .setPose(estimatedRobotPose.estimatedPose.toPose2d())); + */ + return poseEst; + } + + /** + * The standard deviations of the estimated pose from {@link + * Vision#getEstimatedGlobalPose(Cameras)}, for use with {@link + * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should + * only be used when there are targets visible. + * + * @param camera Desired camera to get the standard deviation of the estimated pose. + */ + public Matrix getEstimationStdDevs(Cameras camera) { + var poseEst = getEstimatedGlobalPose(camera); + var estStdDevs = camera.singleTagStdDevs; + var targets = getLatestResult(camera).getTargets(); + int numTags = 0; + double avgDist = 0; + for (var tgt : targets) { + var tagPose = camera.poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) { + continue; + } + numTags++; + if (poseEst.isPresent()) { + avgDist += + PhotonUtils.getDistanceToPose( + poseEst.get().estimatedPose.toPose2d(), tagPose.get().toPose2d()); + } + } + if (numTags == 0) { + return estStdDevs; + } + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) { + estStdDevs = camera.multiTagStdDevs; + } + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) { + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + } else { + estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + } + + return estStdDevs; + } + + /** + * Filter pose via the ambiguity and find best estimate between all of the camera's throwing out + * distances more than 10m for a short amount of time. + * + * @param pose Estimated robot pose. + * @return Could be empty if there isn't a good reading. + */ + private Optional filterPose(Optional pose) { + if (pose.isPresent()) { + double bestTargetAmbiguity = 1; // 1 is max ambiguity + for (PhotonTrackedTarget target : pose.get().targetsUsed) { + double ambiguity = target.getPoseAmbiguity(); + if (ambiguity != -1 && ambiguity < bestTargetAmbiguity) { + bestTargetAmbiguity = ambiguity; + } + } + // ambiguity to high dont use estimate + if (bestTargetAmbiguity > maximumAmbiguity) { + return Optional.empty(); + } + + // est pose is very far from recorded robot pose + if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) + > 1) { + longDistangePoseEstimationCount++; + + // if it calculates that were 10 meter away for more than 10 times in a row its probably + // right + if (longDistangePoseEstimationCount < 10) { + return Optional.empty(); + } + } else { + longDistangePoseEstimationCount = 0; + } + return pose; + } + return Optional.empty(); + } + + /** + * Get the latest result from a given Camera. + * + * @param camera Given camera to take the result from. + * @return Photon result from sim or a real camera. + */ + public PhotonPipelineResult getLatestResult(Cameras camera) { + + return Robot.isReal() + ? camera.camera.getLatestResult() + : camera.cameraSim.getCamera().getLatestResult(); + } + + /** + * Get distance of the robot from the AprilTag pose. + * + * @param id AprilTag ID + * @return Distance + */ + public double getDistanceFromAprilTag(int id) { + Optional tag = fieldLayout.getTagPose(id); + return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())) + .orElse(-1.0); + } + + /** + * Get tracked target from a camera of AprilTagID + * + * @param id AprilTag ID + * @param camera Camera to check. + * @return Tracked target. + */ + public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) { + PhotonTrackedTarget target = null; + PhotonPipelineResult result = getLatestResult(camera); + if (result.hasTargets()) { + for (PhotonTrackedTarget i : result.getTargets()) { + if (i.getFiducialId() == id) { + target = i; + } + } + } + return target; + } + + /** + * Vision simulation. + * + * @return Vision Simulation + */ + public VisionSystemSim getVisionSim() { + return visionSim; + } + + /** + * Open up the photon vision camera streams on the localhost, assumes running photon vision on + * localhost. + */ + private void openSimCameraViews() { + if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) { + // try + // { + // Desktop.getDesktop().browse(new URI("http://localhost:1182/")); + // Desktop.getDesktop().browse(new URI("http://localhost:1184/")); + // Desktop.getDesktop().browse(new URI("http://localhost:1186/")); + // } catch (IOException | URISyntaxException e) + // { + // e.printStackTrace(); + // } + } + } + + /** Update the {@link Field2d} to include tracked targets/ */ + public void updateVisionField() { + + List targets = new ArrayList(); + for (Cameras c : Cameras.values()) { + if (getLatestResult(c).hasTargets()) { + targets.addAll(getLatestResult(c).targets); + } + } + + List poses = new ArrayList<>(); + for (PhotonTrackedTarget target : targets) { + if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) { + Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d(); + poses.add(targetPose); + } + } + + field2d.getObject("tracked targets").setPoses(poses); + } + + /** Camera Enum to select each camera */ + enum Cameras { + /** Left Camera */ + LEFT_CAM( + "left", + new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)), + new Translation3d( + Units.inchesToMeters(12.056), Units.inchesToMeters(10.981), Units.inchesToMeters(8.44)), + VecBuilder.fill(4, 4, 8), + VecBuilder.fill(0.5, 0.5, 1)), + /** Right Camera */ + RIGHT_CAM( + "right", + new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)), + new Translation3d( + Units.inchesToMeters(12.056), + Units.inchesToMeters(-10.981), + Units.inchesToMeters(8.44)), + VecBuilder.fill(4, 4, 8), + VecBuilder.fill(0.5, 0.5, 1)), + /** Center Camera */ + CENTER_CAM( + "center", + new Rotation3d(0, Units.degreesToRadians(18), 0), + new Translation3d( + Units.inchesToMeters(-4.628), + Units.inchesToMeters(-10.687), + Units.inchesToMeters(16.129)), + VecBuilder.fill(4, 4, 8), + VecBuilder.fill(0.5, 0.5, 1)); + + /** Latency alert to use when high latency is detected. */ + public final Alert latencyAlert; + /** Camera instance for comms. */ + public final PhotonCamera camera; + /** Pose estimator for camera. */ + public final PhotonPoseEstimator poseEstimator; + + public final Matrix singleTagStdDevs; + public final Matrix multiTagStdDevs; + /** Transform of the camera rotation and translation relative to the center of the robot */ + private final Transform3d robotToCamTransform; + /** Simulated camera instance which only exists during simulations. */ + public PhotonCameraSim cameraSim; + + /** + * Construct a Photon Camera class with help. Standard deviations are fake values, experiment + * and determine estimation noise on an actual robot. + * + * @param name Name of the PhotonVision camera found in the PV UI. + * @param robotToCamRotation {@link Rotation3d} of the camera. + * @param robotToCamTranslation {@link Translation3d} relative to the center of the robot. + * @param singleTagStdDevs Single AprilTag standard deviations of estimated poses from the + * camera. + * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the + * camera. + */ + Cameras( + String name, + Rotation3d robotToCamRotation, + Translation3d robotToCamTranslation, + Matrix singleTagStdDevs, + Matrix multiTagStdDevsMatrix) { + latencyAlert = + new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.WARNING); + + camera = new PhotonCamera(name); + + // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html + robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation); + + poseEstimator = + new PhotonPoseEstimator( + Vision.fieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + camera, + robotToCamTransform); + poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + + this.singleTagStdDevs = singleTagStdDevs; + this.multiTagStdDevs = multiTagStdDevsMatrix; + + if (Robot.isSimulation()) { + SimCameraProperties cameraProp = new SimCameraProperties(); + // A 640 x 480 camera with a 100 degree diagonal FOV. + cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100)); + // Approximate detection noise with average and standard deviation error in pixels. + cameraProp.setCalibError(0.25, 0.08); + // Set the camera image capture framerate (Note: this is limited by robot loop rate). + cameraProp.setFPS(30); + // The average and standard deviation in milliseconds of image data latency. + cameraProp.setAvgLatencyMs(35); + cameraProp.setLatencyStdDevMs(5); + + cameraSim = new PhotonCameraSim(camera, cameraProp); + cameraSim.enableDrawWireframe(true); + } + } + + /** + * Add camera to {@link VisionSystemSim} for simulated photon vision. + * + * @param systemSim {@link VisionSystemSim} to use. + */ + public void addToVisionSim(VisionSystemSim systemSim) { + if (Robot.isSimulation()) { + systemSim.addCamera(cameraSim, robotToCamTransform); + // cameraSim.enableDrawWireframe(true); + } + } + } +} From 3281d844b0765d4034662f2642990c806b4e72e3 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 26 Sep 2024 08:12:07 -0700 Subject: [PATCH 05/57] Include Choreo as auto option modified: README.md new file: vendordeps/ChoreoLib.json --- README.md | 2 +- vendordeps/ChoreoLib.json | 43 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 44 insertions(+), 1 deletion(-) create mode 100644 vendordeps/ChoreoLib.json diff --git a/README.md b/README.md index 5cd52d31..4690360c 100644 --- a/README.md +++ b/README.md @@ -32,5 +32,5 @@ effective logging for troubleshooting. * [WPILib](https://docs.wpilib.org/en/stable/index.html) -- FIRST basic libraries * [AdvantageKit](https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/docs/WHAT-IS-ADVANTAGEKIT.md) -- Logging * [YAGSL](https://yagsl.gitbook.io/yagsl) -- Swerve drive library -* [PathPlanner](https://pathplanner.dev/home.html) -- Autonomous path planning +* [PathPlanner](https://pathplanner.dev/home.html) / [Choreo](https://sleipnirgroup.github.io/Choreo/) -- Autonomous path planning * [PhotonVision](https://docs.photonvision.org/en/latest/) -- Robot vision / tracking diff --git a/vendordeps/ChoreoLib.json b/vendordeps/ChoreoLib.json new file mode 100644 index 00000000..7d6e9571 --- /dev/null +++ b/vendordeps/ChoreoLib.json @@ -0,0 +1,43 @@ +{ + "fileName": "ChoreoLib.json", + "name": "ChoreoLib", + "version": "2024.2.3", + "uuid": "287cff6e-1b60-4412-8059-f6834fb30e30", + "frcYear": "2024", + "mavenUrls": [ + "https://SleipnirGroup.github.io/ChoreoLib/dep" + ], + "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib.json", + "javaDependencies": [ + { + "groupId": "com.choreo.lib", + "artifactId": "ChoreoLib-java", + "version": "2024.2.3" + }, + { + "groupId": "com.google.code.gson", + "artifactId": "gson", + "version": "2.10.1" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.choreo.lib", + "artifactId": "ChoreoLib-cpp", + "version": "2024.2.3", + "libName": "ChoreoLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} From 3294049504dd0b05eb42c958ddb47a9e397f682b Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sat, 28 Sep 2024 13:02:19 -0700 Subject: [PATCH 06/57] Add logo to README new file: AZ-First-logo.png modified: README.md --- AZ-First-logo.png | Bin 0 -> 9786 bytes README.md | 2 ++ 2 files changed, 2 insertions(+) create mode 100644 AZ-First-logo.png diff --git a/AZ-First-logo.png b/AZ-First-logo.png new file mode 100644 index 0000000000000000000000000000000000000000..afb5f52a99a5edb9ad256592c745c668b2db96c1 GIT binary patch literal 9786 zcmdUVWm6o$vi9Qc?iNT0iwAcL5G+B$;)_dg0%UQA;O=gLV9O%Gg8SkmxI4jZ$wGja zbML9Tzu}##p68kAuActVQ$17t^}VJt0WK{r001CRRZ-M_Mw4fGkB#{}`<_7}{U+ z|1(4_0e}Ov|HJ1~6R`m3&l)}fem<0>|K%vp9sq^^-RI}yLn-`Uk55kk0BYg?di+n+ zX#eH!|Ks|3G!G@UQs4B|oe#t#C#`>bGwKfq9gMseYQ?V%@Mor*~wgPNT#|zm*m{ z+dS-fPgA*&`KdeS$*K(j{ z)uO2aF5Uy_wMEY*A1V_*di{zoEt# zesEbwiKO*iw$EFoKr4?t5)pUi|ZwI z-~E_u#REAjrR#tp)oXjvxKlD%MGSWIOB3leUPM%e(wVp@2A=bgP}~ z8AK;=&7t`>^8zKG6{It1H@J9f?{a}25!C?#{Mtv==Nd^d zFT-{4)b}+;gd_%SjB>Z#qbz}j!bYs;Cu?5E3&B7U9B%!dfOB%bbY-a`;#IHPpEAd1 z_Hn;((mkeV&WFFCKk$K~6KOn*yuP!QqonX&0fvz~|I{wkv0;Rtg#|%p0B}P14V>h& z^@;-mv+>iiVG~79GxpRxje#l1c&6jFwEI|GVm9txvbH06=o=dqgOY&Yeg$!JyW z;%o(gn*riOxqxc}XfAp)^3VMqD%{E3N=(4m zvJF(Xug!Mje_ZL#LQ;zNtu1)G6+1gI&XFvWgAVZ78`Omh1RyE_hD#*SH!D=u`&2($ zQk?Ykexr9#j9luu88YIAuha0+gPbjaw$8=q5(Y`#Y|Ct@*ZQBFUPHR(#dd;@_S+M) zy0|#dcyF0iu&ZfkUZdNL*;!?M}Eb3&x6-MH?o8@`1>uhro-#W^RfhBPf&IkB=rM<4w8uyK38w^q@yX zAxWgHVM{t&ia7-8E8u?;y*{1iy+$DgIBHVgr&%tc|9m3|eloaO$>Gn#UiLg3W|J2l zdz`;dzNW=}o=_{9jY zNhkp1eCTaV3bOK03Y7cYF%~3VQQ*W9q7?#!pBPcnnXlN55-zb)9%!n)-x~BrE;~Ef zj%$_-!QNZ8wgW(>lmVX(D*84!2GMYdHr7jR4+P#x>|lhLeH{0V5=Ow3c0)<>UWWu= zgU(RVHca-0G5m2rABXzAUor7p zOH$v*pD19=4uqNyQN>*4EK^*6|NgpicR$3R%it6xUFKrgGLr626dy=siU+tWW0Uf@ z8aw(b;d|rA^hLxamYPm_icRbzm$)I?vRWA)@^^UXaPl`Zj@uBBHNp}hP>{e5fF9CdX)wtCY!!|rP>f*(y9g1&t=m7o*XjwgBDAkOIGB8Rbx0S=E zPpT6%Q$djmk4dE-ybFaoD;=VGH$2RcPX%W~>9G(-fPZQl@KCun`^OMA0{>h$U`8;p z$^iCQ(OD-VVzvXxxHWDdF`@rK?$+4H`Wk#&3aVjapAPrKV8mV&yn6fhw?_|V@}o}1 zPJy6>s^V}^YFt!eh-e`N%A3;^Ne8{^tM}+Wa%18)=8iLCxx*$`m%WhF;)Sb8$C4jo zH&e5bKns_kYm5HAmAD|T?S|z^y_HU<_N3k027z2F2n4coleA65bkJjv&rBsWykwXI zzn;HH`MSO>HpN4uaP=kj%=#b zPnIuO$A6T>;W5kFs;LmYzV;M(Kj`%cjU9S8T1Ck+X$G39NBiZAnVR98WHlAyG=Wm$ z+>8r8MEcs)MCA#8ajTw9MApZSs1?t;m@wscjZty(5KfP$(c2Zw`!;r;AmK|*t!ht* zKefSmkP*w|YR-D$ALvOfL&;{fo~6Uj)4Z{xk)E2NxG0(VS}fo2$FfdJ!CC8S z-q|4?F8c70%h`*RS_8#48y-U*KLiwD+?H_`?5Q}lJpQS&)}n{l@Fg@s6fQ*~0`0IE znJH=NcDIZKeQWS!)mm)>eum`_FX6hx$;cF>J{Kj6oui|ZKUV#D!y_pv3LCch@oQZtRa4b!&1OC(c3Z#gs4w<^XQGyzIP_R>L^7PW|1Bt)w)H(F}3<=R4 zM7XGEt1Y`ms@j^m$(8?de-_HlBjQkl?e9uyy> zmfjU5M*dgb&IVp-hz(PbGwrnAj)?1T@HqjQLB#y6SFFb1b|_>)6Qh1ulYb0MK-!nC zrx9^+baVTd?jBO})$8vQ$HNy zF;6Pd$VsGNbq9XyAvmF62`ueqDlFsrDD0ne{0`j8!JToav~ONhz!dZvK;wQGtF}Fj_S3d$9y2FKYd)y3D7`zrQ_f%bHv@p<2h5Rg1-#+Uy|xWi<4((YS*Ej&lUSN1!;LV0f%Ooqnd zT&mV0+`)+5_MUL#%@EC-$o+2`-`r4A^sJM0mf^^0qOe@Tx6P9O>>_81NoVK&)TpCg zpfJ{%7pw0$xk9qQkY9;ajjtDF(RpXR0z(nIewuzLuJ zunPah-qSk!JEiUA=Lh^k7nMZHk;|?MvbpTkiz?Nw7>_{0u$Swk4h8d3^lIpldH}ek zFHhH>3b2?47)hrrSoZ>J$n4!82JUY*aRlq-|3dr_U@j_dRtk@OuNyt*yW5GyAFRtI zO+IoGhLawrSep%GOn-LSuHZ zGb98O)tD~-OXF@@tY}w96+M`@12V0--zblwylv^FuqOHexF;+i3Ye5&15uZr-7*j_ zuFq0d-Me+`#a6BlcjFn9ekm@K;;)t`7XpcCzRe#jv^&;{lfPhj*uhHczI=LTg}l<& zka*pTP)zS4SkIzM(-8&Hp*bXI^%^|T9El}Z<4Gd-|UiNR35pk)I;SFS5 zh{=at9*`wSzB@|EzX49!IN$*!iGw=lbIt;2JFEJ7BdL1513&P$6(*VdSX;Xu%)}WlXuDIQ^IhInCXC`2 zj9AaDR9#%KObGAy6AeL0NAEt($QlXL<>3OAYON9L;|<_%E4eww*BSO8#ZeZrpq+${ zk^$bm1ZGdNEa=qEL_bg`V`$+&r43>HBLA`kO|<%O^TOX31gIpt#FLPHC2kM{QExaR z{zmcgc1bKyFw@X&N}i$gm0%PR-=Iv%C1f~5cBc3CzdaS%yF3+bx;S!vsD8n4U>_&s zJ{g`R?56-*{S$2=M>i)^+w|Rbj?otQB7;&4o$WsRuW^hi6yPyhh^DD;4E9S9UAt7S23E& zs(S!$cK6MX&id-VJAWA-9`23NjY=p2F zlm9%EWJxa>LS@KT#b@++m(zv{LJ9dZxTyRj z?(TPGJJCsUjQ&9w$`@S7J{s|yB1zDRBouC%u{SxvzJ)v^$leB#)tQ$2!No?Fk)i?x zCY0FwxWwO_mLUWZa-mf%Pzrq#Wl~901o^@(4vb_rSeQ_8G(C;Wa#~`>dO_OpM#eRuB?tkD*8OHgtFZzP&;htnEor`SIT*afuA4`W2~wmK6$ zY@5*KOY^`DI7novzSh*Zku`R#4G&dPg@r#3Dq?*nfV>3|UN^cCv3(HUDhn<17t2|k z!FCs1$N_WpaCA2}w7ymNr7BK5xnGb}Q}WUakrpPs)(9r4IogimD{NYyF5Z-}d;U*g z=H#CFsfHoZ&(13GI?_nx>$Eb(PQRhmn+yU3*e4c5(tvMB-GraNo-1tcHrr@4H>;fS z#ojz<7kShf;L`rEP^iGSGrjL`GuY4}*?cs(IBp@Ecn;U2#j+`Ov|IjyUWvxdrb=+^ z50>cO3(8YgUXB99{_^M^2dCGmcx8@S$}tYZb=aig%cvO= zB7r3=uqm_(P`yTK)kvhi^j9f2&b+uJk`#F13cFowT9`CUN#GB&g7yBT57i*{#DEJu%mEly--F$X|rmELlGE=!; z@$VNUG5=zLYwL<(Go1XjYIgYiqKvOC9Vg)z14B0NKDX~0?zF5b@D23z*}Mi9rSzjL z=?PN@00>?(DW?5IkDEC0m|jfG-`e?vb-{uKxb-X85~o}>3pjK$BNB! zCb_)I*^tWLsxn~^UQZLx zexIgG@9SJ{8IkQu%NB7q1kV{(p|JpXI4iO5IlYcqeXgsP>RRRA)?Vi~eAtfK>^h^J1YZoJ^YN+Y=#Zvw+NcdOH?!&0*SAw4_R#n!1-(Ox??%6x`Mm z&`Bu_YWY@J{@x?L@I|o`vDj}-A+6t-#&K69`^pB;BJ~ox% zvMc!jSwMeMobW=EZYSxe*O+AfzO(^*_%krQ_7+({-v1_Vn8Ts9>Jx<)Xx`a_;Sv5s zqv}QTwr=-$7ug;mJ)keDNkHlS)1`-^E5BsW)rt~=JeEX5#E`eLw?@g`wcwcWLYmEb~u;|IOuFDC$X=2m!7Yda-W4% z_XIHHRr5*y^ZT~hj^3m6rG<_jx^cnkim>dEiA*^V7w@Y~S5U{a#Of9oci`KoZRcP{O-_c`dcV-=`!|nM*pLnS-xl)r?Y63O8RpN z+f@?wdC}wK#_IMj=ibbW0A4zN!jb(yZ2~j8xLw8sR5(*dc6#JNv&tr8{mH zGuUAH5k462l~(@GOZJWCZ0*;k-b2@56HF# zPYcac9F@c9w4HVzPD)n9)i6TFd(8h67Elr%Pj69V8G$Y0 zwZkc&LOooP9mLF$p~K&LjH{|+>=zk^V+jAd!QupSx*i{!CJ8NoI~1Uv>(d2`5fnzs zgzs{mLy12hL54Mzl>soz6&O3&va#1oBh&*9^i}JI?B6>(vpwQ%-5c%k(c|W*Y(Lxb zd~v}}t!3@n0K1nTj=c~YN%YZqDGJ%}y%*HhBlb~JQW2cH7BDCDmG6q_YE!lc_A@E> zpg*oKnW${r_+_7;J&gz>;mSpYpRP z34i&{8A!4Fk4B%?L69yw9FwX?t^n)praY(7=#3@KI&TKknJ7d06Q0@vW`tVK&v&`Y z$EbS@RV*k|hIfoi)Ph(d$6AI<`dF*>q+Px|3!lEPKC*Op+ST>#JI6i-%h=B z`OM8*oU_Lp`-`3PD=@=(i(4G7wWjg;f^f3>5^p7p8I>!$n-$84vC)Zz6a{!eF_wvj z!h33oMZTa&m}Y)qzKwGF3)bKy{D$#n4!_%=TU|U;vtc1pDwjO`kk3v4ZIX5EtzeKX zWknx3%djOY!LSJf2h4?K~?*BFv+-Zw59qD}i$NHn{$%jIj zDRun}ZS?}s5GjquK@-=M07){QEh)EPbCiXBS4%XCsy>XPL$EzJNPMOL@U^Wb6%^-u z6&(~Gy36i*%J?M!-I(HTR46@XlY3Sp1q0J@rMbn!ZjVoGt0H2ZWGFqxSNz4PecE>y zHfmx|wC*QvgVVhfC$^?xcdi0cZQB^GcPn+iK$(*+SFwX*RN`(vV<^cr!*Ot-tpx>R z^pyO+>URn3i!43bU@`R0c5FxDplN;21%kXR6AB`0@@g%Bn&psz)9^|qn5$bjD6b^+ z_y7Zb6doqUvhyALx!B>_@cY@SJb)0oD3dagyygV2S6(QN22Y}ObM)K+tdLG2yVUsM z{uF6R`a@dIq)*t{AS9$+*x)2$8;vfN?7k$2QZ91dWM$(FgN-#hAo!|3?o+v)8iAwW zH6F*bh-6j28e9U^u>ow>l$c+!5Fl9nvhPwuNv51+!k_1!fJfsd!fDV$?VhljI;bm3 zO7c-+$rJi;5>K5%b1s3jd zLiG0h*GLBbzfpHPyAp&Uw)Yx=~x?l)cvVNs&8Orq3*&PszbG(9kK7up#ZRL@(i*}-nasW z-zvo6X5dMl0vCj7(#fWcNaoqrS2B7Adw!}@x_c4Q^-u(D3}+N*xfl2b`zlU$X{YM?ikrYom^Uh8p5w8D z)<*Im8b3M$SJnd49*OEmWxV0B#SO+RxVQ<>gO0t@!q}p}#FUAkJ1ad9u}kH}DOK*o zS>WZHq8Hx!3}rr2st@cx-`i;N$8BbC#IgHZde7TI6Fl^w6)G}$K%gN^&wF<0ph|uW zZv>w#j=qCA&1>2`<+~9Su`z&$2ww*X9QeExA94-8)a!@|-qhZbcprJ7TeD*6dZW3S zA9sLH0MO%V3ygK#rCU!cMsXq1kw@m;bXn?C^j>Jo7w)&G8Ckp-M# zXMT^8-b)4ebfhOG7o(tVP`sABa}2It)x61Wn&g65vub^% zg|Xg-!}?N}GY(y%xhHPtf-ToAzx^b}UvHVD7{4-iu~z(DvITUDpY4Z(yL9_}J>fv-8R(g9ZP8`9Qv=ugrIlbFX0isO(@pmx z>@VquU0F!pp6hHgVfr>q1uApq5;dFUUPXx(mn7P0+=D_Ouql7{*}{F;MM4SWK!of2Atpepp zH*om?zwQI!Vv$B%U?@CdH7uB{Rw5YRenkE8aX52)W*ia7MGag8)qovyLh}cus6xfU z<~OEbF8J4$pWk3k5zutC0EVoFDIJ!t$C%Zw9QGAlaM{&Lv$+S=Hwrz*zA&l_eh$6L z-BxeT&&y4cryu!(a>nc(?&{p-!*v}Kp2s_H43 zzL(mc=@fjQf9A=*8Zri~y|ZV*f!qEU>sCZPtU5nwEAeY<@r*ajjN}y%4X;T8p&W9U zl&woC&&@#$b@h?mF?1YmgdaWMY1!w!?`k$%e(FL58qt)<1$Y*YJU1i#|K3amP*u`Y Jtd$3c{67@CaFzf7 literal 0 HcmV?d00001 diff --git a/README.md b/README.md index 4690360c..7be4194f 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,5 @@ +![AzFIRST Logo](https://github.com/AZ-First/Az-RBSI/blob/main/AZ-First-logo.png?raw=true) + # Az-RBSI Arizona's Reference Build and Software Implementation for FRC Robots (read: "A-Z-ribsy") From 0e0194172d21569c8b59f6bf063c94f099255f63 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sun, 29 Sep 2024 22:23:26 -0700 Subject: [PATCH 07/57] GitHub dependency checks new file: .github/workflows/dependency-submission.yml --- .github/workflows/dependency-submission.yml | 22 +++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 .github/workflows/dependency-submission.yml diff --git a/.github/workflows/dependency-submission.yml b/.github/workflows/dependency-submission.yml new file mode 100644 index 00000000..2660b3ef --- /dev/null +++ b/.github/workflows/dependency-submission.yml @@ -0,0 +1,22 @@ +name: Dependency Submission + +on: + push: + branches: [ 'main' ] + +permissions: + contents: write + +jobs: + dependency-submission: + runs-on: ubuntu-latest + steps: + - name: Checkout sources + uses: actions/checkout@v4 + - name: Setup Java + uses: actions/setup-java@v4 + with: + distribution: 'temurin' + java-version: 17 + - name: Generate and submit dependency graph + uses: gradle/actions/dependency-submission@v4 From 7be4c8f6ce5fb220d8c207b6846c837029312956 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 2 Oct 2024 10:20:50 -0700 Subject: [PATCH 08/57] Remove drive subsystem and commands from AK sketch Since we are going to use the YAGSL swerve library -- with its associated example commands and subsystem -- remove the subsystem and commands that came with the AdvantageKit sketch. modified: src/main/java/frc/robot/Robot.java deleted: src/main/java/frc/robot/commands/DriveCommands.java deleted: src/main/java/frc/robot/subsystems/drive/Drive.java deleted: src/main/java/frc/robot/subsystems/drive/GyroIO.java deleted: src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java deleted: src/main/java/frc/robot/subsystems/drive/Module.java deleted: src/main/java/frc/robot/subsystems/drive/ModuleIO.java deleted: src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java deleted: src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java deleted: src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java deleted: src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java deleted: src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java --- src/main/java/frc/robot/Robot.java | 6 +- .../frc/robot/commands/DriveCommands.java | 77 ----- .../frc/robot/subsystems/drive/Drive.java | 299 ------------------ .../frc/robot/subsystems/drive/GyroIO.java | 30 -- .../robot/subsystems/drive/GyroIOPigeon2.java | 75 ----- .../frc/robot/subsystems/drive/Module.java | 204 ------------ .../frc/robot/subsystems/drive/ModuleIO.java | 52 --- .../robot/subsystems/drive/ModuleIOSim.java | 72 ----- .../subsystems/drive/ModuleIOSparkMax.java | 202 ------------ .../subsystems/drive/ModuleIOTalonFX.java | 218 ------------- .../drive/PhoenixOdometryThread.java | 138 -------- .../drive/SparkMaxOdometryThread.java | 107 ------- 12 files changed, 5 insertions(+), 1475 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/DriveCommands.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/Drive.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIO.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/Module.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIO.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 679551dc..1a1ee3d5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,4 +1,6 @@ -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright (c) 2021-2024 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -121,6 +123,7 @@ public void robotPeriodic() { /** This function is called once when the robot is disabled. */ @Override public void disabledInit() { + // Set the brakes to stop robot motion robotContainer.setMotorBrake(true); disabledTimer.reset(); disabledTimer.start(); @@ -129,6 +132,7 @@ public void disabledInit() { /** This function is called periodically when disabled. */ @Override public void disabledPeriodic() { + // After WHEEL_LOCK_TIME has elapsed, release the drive brakes if (disabledTimer.hasElapsed(Constants.DrivebaseConstants.WHEEL_LOCK_TIME)) { robotContainer.setMotorBrake(false); disabledTimer.stop(); diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java deleted file mode 100644 index fc8f596b..00000000 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.commands; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.subsystems.drive.Drive; -import java.util.function.DoubleSupplier; - -public class DriveCommands { - private static final double DEADBAND = 0.1; - - private DriveCommands() {} - - /** - * Field relative drive command using two joysticks (controlling linear and angular velocities). - */ - public static Command joystickDrive( - Drive drive, - DoubleSupplier xSupplier, - DoubleSupplier ySupplier, - DoubleSupplier omegaSupplier) { - return Commands.run( - () -> { - // Apply deadband - double linearMagnitude = - MathUtil.applyDeadband( - Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND); - Rotation2d linearDirection = - new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); - double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); - - // Square values - linearMagnitude = linearMagnitude * linearMagnitude; - omega = Math.copySign(omega * omega, omega); - - // Calcaulate new linear velocity - Translation2d linearVelocity = - new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) - .getTranslation(); - - // Convert to field relative speeds & send command - boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; - drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega * drive.getMaxAngularSpeedRadPerSec(), - isFlipped - ? drive.getRotation().plus(new Rotation2d(Math.PI)) - : drive.getRotation())); - }, - drive); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java deleted file mode 100644 index 51d02c0e..00000000 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ /dev/null @@ -1,299 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.subsystems.drive; - -import static edu.wpi.first.units.Units.*; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.pathfinding.Pathfinding; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PathPlannerLogging; -import com.pathplanner.lib.util.ReplanningConfig; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.util.LocalADStarAK; -import java.util.concurrent.locks.Lock; -import java.util.concurrent.locks.ReentrantLock; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -public class Drive extends SubsystemBase { - private static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5); - private static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); - private static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); - private static final double DRIVE_BASE_RADIUS = - Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); - private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; - - static final Lock odometryLock = new ReentrantLock(); - private final GyroIO gyroIO; - private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); - private final Module[] modules = new Module[4]; // FL, FR, BL, BR - private final SysIdRoutine sysId; - - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); - private Rotation2d rawGyroRotation = new Rotation2d(); - private SwerveModulePosition[] lastModulePositions = // For delta tracking - new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }; - private SwerveDrivePoseEstimator poseEstimator = - new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); - - public Drive( - GyroIO gyroIO, - ModuleIO flModuleIO, - ModuleIO frModuleIO, - ModuleIO blModuleIO, - ModuleIO brModuleIO) { - this.gyroIO = gyroIO; - modules[0] = new Module(flModuleIO, 0); - modules[1] = new Module(frModuleIO, 1); - modules[2] = new Module(blModuleIO, 2); - modules[3] = new Module(brModuleIO, 3); - - // Start threads (no-op for each if no signals have been created) - PhoenixOdometryThread.getInstance().start(); - SparkMaxOdometryThread.getInstance().start(); - - // Configure AutoBuilder for PathPlanner - AutoBuilder.configureHolonomic( - this::getPose, - this::setPose, - () -> kinematics.toChassisSpeeds(getModuleStates()), - this::runVelocity, - new HolonomicPathFollowerConfig( - MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), - () -> - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red, - this); - Pathfinding.setPathfinder(new LocalADStarAK()); - PathPlannerLogging.setLogActivePathCallback( - (activePath) -> { - Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); - }); - PathPlannerLogging.setLogTargetPoseCallback( - (targetPose) -> { - Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); - }); - - // Configure SysId - sysId = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - null, - null, - (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), - new SysIdRoutine.Mechanism( - (voltage) -> { - for (int i = 0; i < 4; i++) { - modules[i].runCharacterization(voltage.in(Volts)); - } - }, - null, - this)); - } - - public void periodic() { - odometryLock.lock(); // Prevents odometry updates while reading data - gyroIO.updateInputs(gyroInputs); - for (var module : modules) { - module.updateInputs(); - } - odometryLock.unlock(); - Logger.processInputs("Drive/Gyro", gyroInputs); - for (var module : modules) { - module.periodic(); - } - - // Stop moving when disabled - if (DriverStation.isDisabled()) { - for (var module : modules) { - module.stop(); - } - } - // Log empty setpoint states when disabled - if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); - } - - // Update odometry - double[] sampleTimestamps = - modules[0].getOdometryTimestamps(); // All signals are sampled together - int sampleCount = sampleTimestamps.length; - for (int i = 0; i < sampleCount; i++) { - // Read wheel positions and deltas from each module - SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; - SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; - for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { - modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; - moduleDeltas[moduleIndex] = - new SwerveModulePosition( - modulePositions[moduleIndex].distanceMeters - - lastModulePositions[moduleIndex].distanceMeters, - modulePositions[moduleIndex].angle); - lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; - } - - // Update gyro angle - if (gyroInputs.connected) { - // Use the real gyro angle - rawGyroRotation = gyroInputs.odometryYawPositions[i]; - } else { - // Use the angle delta from the kinematics and module deltas - Twist2d twist = kinematics.toTwist2d(moduleDeltas); - rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); - } - - // Apply update - poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); - } - } - - /** - * Runs the drive at the desired velocity. - * - * @param speeds Speeds in meters/sec - */ - public void runVelocity(ChassisSpeeds speeds) { - // Calculate module setpoints - ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); - SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED); - - // Send setpoints to modules - SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - // The module returns the optimized state, useful for logging - optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); - } - - // Log setpoint states - Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); - } - - /** Stops the drive. */ - public void stop() { - runVelocity(new ChassisSpeeds()); - } - - /** - * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will - * return to their normal orientations the next time a nonzero velocity is requested. - */ - public void stopWithX() { - Rotation2d[] headings = new Rotation2d[4]; - for (int i = 0; i < 4; i++) { - headings[i] = getModuleTranslations()[i].getAngle(); - } - kinematics.resetHeadings(headings); - stop(); - } - - /** Returns a command to run a quasistatic test in the specified direction. */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysId.quasistatic(direction); - } - - /** Returns a command to run a dynamic test in the specified direction. */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysId.dynamic(direction); - } - - /** Returns the module states (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "SwerveStates/Measured") - private SwerveModuleState[] getModuleStates() { - SwerveModuleState[] states = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - states[i] = modules[i].getState(); - } - return states; - } - - /** Returns the module positions (turn angles and drive positions) for all of the modules. */ - private SwerveModulePosition[] getModulePositions() { - SwerveModulePosition[] states = new SwerveModulePosition[4]; - for (int i = 0; i < 4; i++) { - states[i] = modules[i].getPosition(); - } - return states; - } - - /** Returns the current odometry pose. */ - @AutoLogOutput(key = "Odometry/Robot") - public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); - } - - /** Returns the current odometry rotation. */ - public Rotation2d getRotation() { - return getPose().getRotation(); - } - - /** Resets the current odometry pose. */ - public void setPose(Pose2d pose) { - poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); - } - - /** - * Adds a vision measurement to the pose estimator. - * - * @param visionPose The pose of the robot as measured by the vision camera. - * @param timestamp The timestamp of the vision measurement in seconds. - */ - public void addVisionMeasurement(Pose2d visionPose, double timestamp) { - poseEstimator.addVisionMeasurement(visionPose, timestamp); - } - - /** Returns the maximum linear speed in meters per sec. */ - public double getMaxLinearSpeedMetersPerSec() { - return MAX_LINEAR_SPEED; - } - - /** Returns the maximum angular speed in radians per sec. */ - public double getMaxAngularSpeedRadPerSec() { - return MAX_ANGULAR_SPEED; - } - - /** Returns an array of module translations. */ - public static Translation2d[] getModuleTranslations() { - return new Translation2d[] { - new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), - new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0), - new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), - new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0) - }; - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java deleted file mode 100644 index 18ce6fde..00000000 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.subsystems.drive; - -import edu.wpi.first.math.geometry.Rotation2d; -import org.littletonrobotics.junction.AutoLog; - -public interface GyroIO { - @AutoLog - public static class GyroIOInputs { - public boolean connected = false; - public Rotation2d yawPosition = new Rotation2d(); - public double[] odometryYawTimestamps = new double[] {}; - public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; - public double yawVelocityRadPerSec = 0.0; - } - - public default void updateInputs(GyroIOInputs inputs) {} -} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java deleted file mode 100644 index 01ccef28..00000000 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.subsystems.drive; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Pigeon2Configuration; -import com.ctre.phoenix6.hardware.Pigeon2; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import java.util.OptionalDouble; -import java.util.Queue; - -/** IO implementation for Pigeon2 */ -public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(20); - private final StatusSignal yaw = pigeon.getYaw(); - private final Queue yawPositionQueue; - private final Queue yawTimestampQueue; - private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); - - public GyroIOPigeon2(boolean phoenixDrive) { - pigeon.getConfigurator().apply(new Pigeon2Configuration()); - pigeon.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(Module.ODOMETRY_FREQUENCY); - yawVelocity.setUpdateFrequency(100.0); - pigeon.optimizeBusUtilization(); - if (phoenixDrive) { - yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(pigeon, pigeon.getYaw()); - } else { - yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal( - () -> { - boolean valid = yaw.refresh().getStatus().isOK(); - if (valid) { - return OptionalDouble.of(yaw.getValueAsDouble()); - } else { - return OptionalDouble.empty(); - } - }); - } - } - - @Override - public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); - inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); - - inputs.odometryYawTimestamps = - yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryYawPositions = - yawPositionQueue.stream() - .map((Double value) -> Rotation2d.fromDegrees(value)) - .toArray(Rotation2d[]::new); - yawTimestampQueue.clear(); - yawPositionQueue.clear(); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java deleted file mode 100644 index 08eef971..00000000 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ /dev/null @@ -1,204 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.subsystems.drive; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import frc.robot.Constants; -import org.littletonrobotics.junction.Logger; - -public class Module { - private static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); - static final double ODOMETRY_FREQUENCY = 250.0; - - private final ModuleIO io; - private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - private final int index; - - private final SimpleMotorFeedforward driveFeedforward; - private final PIDController driveFeedback; - private final PIDController turnFeedback; - private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop - private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop - private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute - private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; - - public Module(ModuleIO io, int index) { - this.io = io; - this.index = index; - - // Switch constants based on mode (the physics simulator is treated as a - // separate robot with different tuning) - switch (Constants.getMode()) { - case REAL: - case REPLAY: - driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - driveFeedback = new PIDController(0.05, 0.0, 0.0); - turnFeedback = new PIDController(7.0, 0.0, 0.0); - break; - case SIM: - driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); - driveFeedback = new PIDController(0.1, 0.0, 0.0); - turnFeedback = new PIDController(10.0, 0.0, 0.0); - break; - default: - driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); - driveFeedback = new PIDController(0.0, 0.0, 0.0); - turnFeedback = new PIDController(0.0, 0.0, 0.0); - break; - } - - turnFeedback.enableContinuousInput(-Math.PI, Math.PI); - setBrakeMode(true); - } - - /** - * Update inputs without running the rest of the periodic logic. This is useful since these - * updates need to be properly thread-locked. - */ - public void updateInputs() { - io.updateInputs(inputs); - } - - public void periodic() { - Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); - - // On first cycle, reset relative turn encoder - // Wait until absolute angle is nonzero in case it wasn't initialized yet - if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { - turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); - } - - // Run closed loop turn control - if (angleSetpoint != null) { - io.setTurnVoltage( - turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); - - // Run closed loop drive control - // Only allowed if closed loop turn control is running - if (speedSetpoint != null) { - // Scale velocity based on turn error - // - // When the error is 90°, the velocity setpoint should be 0. As the wheel turns - // towards the setpoint, its velocity should increase. This is achieved by - // taking the component of the velocity in the direction of the setpoint. - double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); - - // Run drive controller - double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; - io.setDriveVoltage( - driveFeedforward.calculate(velocityRadPerSec) - + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); - } - } - - // Calculate positions for odometry - int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together - odometryPositions = new SwerveModulePosition[sampleCount]; - for (int i = 0; i < sampleCount; i++) { - double positionMeters = inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS; - Rotation2d angle = - inputs.odometryTurnPositions[i].plus( - turnRelativeOffset != null ? turnRelativeOffset : new Rotation2d()); - odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); - } - } - - /** Runs the module with the specified setpoint state. Returns the optimized state. */ - public SwerveModuleState runSetpoint(SwerveModuleState state) { - // Optimize state based on current angle - // Controllers run in "periodic" when the setpoint is not null - var optimizedState = SwerveModuleState.optimize(state, getAngle()); - - // Update setpoints, controllers run in "periodic" - angleSetpoint = optimizedState.angle; - speedSetpoint = optimizedState.speedMetersPerSecond; - - return optimizedState; - } - - /** Runs the module with the specified voltage while controlling to zero degrees. */ - public void runCharacterization(double volts) { - // Closed loop turn control - angleSetpoint = new Rotation2d(); - - // Open loop drive control - io.setDriveVoltage(volts); - speedSetpoint = null; - } - - /** Disables all outputs to motors. */ - public void stop() { - io.setTurnVoltage(0.0); - io.setDriveVoltage(0.0); - - // Disable closed loop control for turn and drive - angleSetpoint = null; - speedSetpoint = null; - } - - /** Sets whether brake mode is enabled. */ - public void setBrakeMode(boolean enabled) { - io.setDriveBrakeMode(enabled); - io.setTurnBrakeMode(enabled); - } - - /** Returns the current turn angle of the module. */ - public Rotation2d getAngle() { - if (turnRelativeOffset == null) { - return new Rotation2d(); - } else { - return inputs.turnPosition.plus(turnRelativeOffset); - } - } - - /** Returns the current drive position of the module in meters. */ - public double getPositionMeters() { - return inputs.drivePositionRad * WHEEL_RADIUS; - } - - /** Returns the current drive velocity of the module in meters per second. */ - public double getVelocityMetersPerSec() { - return inputs.driveVelocityRadPerSec * WHEEL_RADIUS; - } - - /** Returns the module position (turn angle and drive position). */ - public SwerveModulePosition getPosition() { - return new SwerveModulePosition(getPositionMeters(), getAngle()); - } - - /** Returns the module state (turn angle and drive velocity). */ - public SwerveModuleState getState() { - return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); - } - - /** Returns the module positions received this cycle. */ - public SwerveModulePosition[] getOdometryPositions() { - return odometryPositions; - } - - /** Returns the timestamps of the samples received this cycle. */ - public double[] getOdometryTimestamps() { - return inputs.odometryTimestamps; - } - - /** Returns the drive velocity in radians/sec. */ - public double getCharacterizationVelocity() { - return inputs.driveVelocityRadPerSec; - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java deleted file mode 100644 index 200afa3a..00000000 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.subsystems.drive; - -import edu.wpi.first.math.geometry.Rotation2d; -import org.littletonrobotics.junction.AutoLog; - -public interface ModuleIO { - @AutoLog - public static class ModuleIOInputs { - public double drivePositionRad = 0.0; - public double driveVelocityRadPerSec = 0.0; - public double driveAppliedVolts = 0.0; - public double[] driveCurrentAmps = new double[] {}; - - public Rotation2d turnAbsolutePosition = new Rotation2d(); - public Rotation2d turnPosition = new Rotation2d(); - public double turnVelocityRadPerSec = 0.0; - public double turnAppliedVolts = 0.0; - public double[] turnCurrentAmps = new double[] {}; - - public double[] odometryTimestamps = new double[] {}; - public double[] odometryDrivePositionsRad = new double[] {}; - public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; - } - - /** Updates the set of loggable inputs. */ - public default void updateInputs(ModuleIOInputs inputs) {} - - /** Run the drive motor at the specified voltage. */ - public default void setDriveVoltage(double volts) {} - - /** Run the turn motor at the specified voltage. */ - public default void setTurnVoltage(double volts) {} - - /** Enable or disable brake mode on the drive motor. */ - public default void setDriveBrakeMode(boolean enable) {} - - /** Enable or disable brake mode on the turn motor. */ - public default void setTurnBrakeMode(boolean enable) {} -} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java deleted file mode 100644 index f2ffe480..00000000 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ /dev/null @@ -1,72 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.subsystems.drive; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; - -/** - * Physics sim implementation of module IO. - * - *

Uses two flywheel sims for the drive and turn motors, with the absolute position initialized - * to a random value. The flywheel sims are not physically accurate, but provide a decent - * approximation for the behavior of the module. - */ -public class ModuleIOSim implements ModuleIO { - private static final double LOOP_PERIOD_SECS = 0.02; - - private DCMotorSim driveSim = new DCMotorSim(DCMotor.getNEO(1), 6.75, 0.025); - private DCMotorSim turnSim = new DCMotorSim(DCMotor.getNEO(1), 150.0 / 7.0, 0.004); - - private final Rotation2d turnAbsoluteInitPosition = new Rotation2d(Math.random() * 2.0 * Math.PI); - private double driveAppliedVolts = 0.0; - private double turnAppliedVolts = 0.0; - - @Override - public void updateInputs(ModuleIOInputs inputs) { - driveSim.update(LOOP_PERIOD_SECS); - turnSim.update(LOOP_PERIOD_SECS); - - inputs.drivePositionRad = driveSim.getAngularPositionRad(); - inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); - inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; - - inputs.turnAbsolutePosition = - new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); - inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); - inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); - inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; - - inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; - inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; - inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; - } - - @Override - public void setDriveVoltage(double volts) { - driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); - driveSim.setInputVoltage(driveAppliedVolts); - } - - @Override - public void setTurnVoltage(double volts) { - turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); - turnSim.setInputVoltage(turnAppliedVolts); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java deleted file mode 100644 index 781ec70b..00000000 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ /dev/null @@ -1,202 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.subsystems.drive; - -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.CANSparkMax; -import com.revrobotics.REVLibError; -import com.revrobotics.RelativeEncoder; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.RobotController; -import java.util.OptionalDouble; -import java.util.Queue; - -/** - * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO - * or NEO 550), and analog absolute encoder connected to the RIO - * - *

NOTE: This implementation should be used as a starting point and adapted to different hardware - * configurations (e.g. If using a CANcoder, copy from "ModuleIOTalonFX") - * - *

To calibrate the absolute encoder offsets, point the modules straight (such that forward - * motion on the drive motor will propel the robot forward) and copy the reported values from the - * absolute encoders using AdvantageScope. These values are logged under - * "/Drive/ModuleX/TurnAbsolutePositionRad" - */ -public class ModuleIOSparkMax implements ModuleIO { - // Gear ratios for SDS MK4i L2, adjust as necessary - private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private static final double TURN_GEAR_RATIO = 150.0 / 7.0; - - private final CANSparkMax driveSparkMax; - private final CANSparkMax turnSparkMax; - - private final RelativeEncoder driveEncoder; - private final RelativeEncoder turnRelativeEncoder; - private final AnalogInput turnAbsoluteEncoder; - private final Queue timestampQueue; - private final Queue drivePositionQueue; - private final Queue turnPositionQueue; - - private final boolean isTurnMotorInverted = true; - private final Rotation2d absoluteEncoderOffset; - - public ModuleIOSparkMax(int index) { - switch (index) { - case 0: - driveSparkMax = new CANSparkMax(1, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(2, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(0); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 1: - driveSparkMax = new CANSparkMax(3, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(4, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(1); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 2: - driveSparkMax = new CANSparkMax(5, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(6, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(2); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 3: - driveSparkMax = new CANSparkMax(7, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(8, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(3); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - default: - throw new RuntimeException("Invalid module index"); - } - - driveSparkMax.restoreFactoryDefaults(); - turnSparkMax.restoreFactoryDefaults(); - - driveSparkMax.setCANTimeout(250); - turnSparkMax.setCANTimeout(250); - - driveEncoder = driveSparkMax.getEncoder(); - turnRelativeEncoder = turnSparkMax.getEncoder(); - - turnSparkMax.setInverted(isTurnMotorInverted); - driveSparkMax.setSmartCurrentLimit(40); - turnSparkMax.setSmartCurrentLimit(30); - driveSparkMax.enableVoltageCompensation(12.0); - turnSparkMax.enableVoltageCompensation(12.0); - - driveEncoder.setPosition(0.0); - driveEncoder.setMeasurementPeriod(10); - driveEncoder.setAverageDepth(2); - - turnRelativeEncoder.setPosition(0.0); - turnRelativeEncoder.setMeasurementPeriod(10); - turnRelativeEncoder.setAverageDepth(2); - - driveSparkMax.setCANTimeout(0); - turnSparkMax.setCANTimeout(0); - - driveSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, (int) (1000.0 / Module.ODOMETRY_FREQUENCY)); - turnSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, (int) (1000.0 / Module.ODOMETRY_FREQUENCY)); - timestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); - drivePositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal( - () -> { - double value = driveEncoder.getPosition(); - if (driveSparkMax.getLastError() == REVLibError.kOk) { - return OptionalDouble.of(value); - } else { - return OptionalDouble.empty(); - } - }); - turnPositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal( - () -> { - double value = turnRelativeEncoder.getPosition(); - if (turnSparkMax.getLastError() == REVLibError.kOk) { - return OptionalDouble.of(value); - } else { - return OptionalDouble.empty(); - } - }); - - driveSparkMax.burnFlash(); - turnSparkMax.burnFlash(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - inputs.drivePositionRad = - Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO; - inputs.driveVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO; - inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); - inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()}; - - inputs.turnAbsolutePosition = - new Rotation2d( - turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V() * 2.0 * Math.PI) - .minus(absoluteEncoderOffset); - inputs.turnPosition = - Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO); - inputs.turnVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) - / TURN_GEAR_RATIO; - inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); - inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; - - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveSparkMax.setVoltage(volts); - } - - @Override - public void setTurnVoltage(double volts) { - turnSparkMax.setVoltage(volts); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java deleted file mode 100644 index b41202ef..00000000 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ /dev/null @@ -1,218 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.subsystems.drive; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import java.util.Queue; - -/** - * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and - * CANcoder - * - *

NOTE: This implementation should be used as a starting point and adapted to different hardware - * configurations (e.g. If using an analog encoder, copy from "ModuleIOSparkMax") - * - *

To calibrate the absolute encoder offsets, point the modules straight (such that forward - * motion on the drive motor will propel the robot forward) and copy the reported values from the - * absolute encoders using AdvantageScope. These values are logged under - * "/Drive/ModuleX/TurnAbsolutePositionRad" - */ -public class ModuleIOTalonFX implements ModuleIO { - private final TalonFX driveTalon; - private final TalonFX turnTalon; - private final CANcoder cancoder; - - private final Queue timestampQueue; - - private final StatusSignal drivePosition; - private final Queue drivePositionQueue; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - - private final StatusSignal turnAbsolutePosition; - private final StatusSignal turnPosition; - private final Queue turnPositionQueue; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; - - // Gear ratios for SDS MK4i L2, adjust as necessary - private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private final double TURN_GEAR_RATIO = 150.0 / 7.0; - - private final boolean isTurnMotorInverted = true; - private final Rotation2d absoluteEncoderOffset; - - public ModuleIOTalonFX(int index) { - switch (index) { - case 0: - driveTalon = new TalonFX(0); - turnTalon = new TalonFX(1); - cancoder = new CANcoder(2); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 1: - driveTalon = new TalonFX(3); - turnTalon = new TalonFX(4); - cancoder = new CANcoder(5); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 2: - driveTalon = new TalonFX(6); - turnTalon = new TalonFX(7); - cancoder = new CANcoder(8); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 3: - driveTalon = new TalonFX(9); - turnTalon = new TalonFX(10); - cancoder = new CANcoder(11); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - default: - throw new RuntimeException("Invalid module index"); - } - - var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; - driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - driveTalon.getConfigurator().apply(driveConfig); - setDriveBrakeMode(true); - - var turnConfig = new TalonFXConfiguration(); - turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; - turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - turnTalon.getConfigurator().apply(turnConfig); - setTurnBrakeMode(true); - - cancoder.getConfigurator().apply(new CANcoderConfiguration()); - - timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - - drivePosition = driveTalon.getPosition(); - drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getSupplyCurrent(); - - turnAbsolutePosition = cancoder.getAbsolutePosition(); - turnPosition = turnTalon.getPosition(); - turnPositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); - turnVelocity = turnTalon.getVelocity(); - turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getSupplyCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll( - Module.ODOMETRY_FREQUENCY, drivePosition, turnPosition); - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); - driveTalon.optimizeBusUtilization(); - turnTalon.optimizeBusUtilization(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); - - inputs.drivePositionRad = - Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; - inputs.driveVelocityRadPerSec = - Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; - - inputs.turnAbsolutePosition = - Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()) - .minus(absoluteEncoderOffset); - inputs.turnPosition = - Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO); - inputs.turnVelocityRadPerSec = - Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO; - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; - - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveTalon.setControl(new VoltageOut(volts)); - } - - @Override - public void setTurnVoltage(double volts) { - turnTalon.setControl(new VoltageOut(volts)); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - driveTalon.getConfigurator().apply(config); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = - isTurnMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - turnTalon.getConfigurator().apply(config); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java deleted file mode 100644 index b7531fac..00000000 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ /dev/null @@ -1,138 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.subsystems.drive; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.hardware.ParentDevice; -import java.util.ArrayList; -import java.util.List; -import java.util.Queue; -import java.util.concurrent.ArrayBlockingQueue; -import java.util.concurrent.locks.Lock; -import java.util.concurrent.locks.ReentrantLock; -import org.littletonrobotics.junction.Logger; - -/** - * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. - * - *

This version is intended for Phoenix 6 devices on both the RIO and CANivore buses. When using - * a CANivore, the thread uses the "waitForAll" blocking method to enable more consistent sampling. - * This also allows Phoenix Pro users to benefit from lower latency between devices using CANivore - * time synchronization. - */ -public class PhoenixOdometryThread extends Thread { - private final Lock signalsLock = - new ReentrantLock(); // Prevents conflicts when registering signals - private BaseStatusSignal[] signals = new BaseStatusSignal[0]; - private final List> queues = new ArrayList<>(); - private final List> timestampQueues = new ArrayList<>(); - private boolean isCANFD = false; - - private static PhoenixOdometryThread instance = null; - - public static PhoenixOdometryThread getInstance() { - if (instance == null) { - instance = new PhoenixOdometryThread(); - } - return instance; - } - - private PhoenixOdometryThread() { - setName("PhoenixOdometryThread"); - setDaemon(true); - } - - @Override - public void start() { - if (timestampQueues.size() > 0) { - super.start(); - } - } - - public Queue registerSignal(ParentDevice device, StatusSignal signal) { - Queue queue = new ArrayBlockingQueue<>(20); - signalsLock.lock(); - Drive.odometryLock.lock(); - try { - isCANFD = CANBus.isNetworkFD(device.getNetwork()); - BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; - System.arraycopy(signals, 0, newSignals, 0, signals.length); - newSignals[signals.length] = signal; - signals = newSignals; - queues.add(queue); - } finally { - signalsLock.unlock(); - Drive.odometryLock.unlock(); - } - return queue; - } - - public Queue makeTimestampQueue() { - Queue queue = new ArrayBlockingQueue<>(20); - Drive.odometryLock.lock(); - try { - timestampQueues.add(queue); - } finally { - Drive.odometryLock.unlock(); - } - return queue; - } - - @Override - public void run() { - while (true) { - // Wait for updates from all signals - signalsLock.lock(); - try { - if (isCANFD) { - BaseStatusSignal.waitForAll(2.0 / Module.ODOMETRY_FREQUENCY, signals); - } else { - // "waitForAll" does not support blocking on multiple - // signals with a bus that is not CAN FD, regardless - // of Pro licensing. No reasoning for this behavior - // is provided by the documentation. - Thread.sleep((long) (1000.0 / Module.ODOMETRY_FREQUENCY)); - if (signals.length > 0) BaseStatusSignal.refreshAll(signals); - } - } catch (InterruptedException e) { - e.printStackTrace(); - } finally { - signalsLock.unlock(); - } - - // Save new data to queues - Drive.odometryLock.lock(); - try { - double timestamp = Logger.getRealTimestamp() / 1e6; - double totalLatency = 0.0; - for (BaseStatusSignal signal : signals) { - totalLatency += signal.getTimestamp().getLatency(); - } - if (signals.length > 0) { - timestamp -= totalLatency / signals.length; - } - for (int i = 0; i < signals.length; i++) { - queues.get(i).offer(signals[i].getValueAsDouble()); - } - for (int i = 0; i < timestampQueues.size(); i++) { - timestampQueues.get(i).offer(timestamp); - } - } finally { - Drive.odometryLock.unlock(); - } - } - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java deleted file mode 100644 index 15db2664..00000000 --- a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.subsystems.drive; - -import edu.wpi.first.wpilibj.Notifier; -import java.util.ArrayList; -import java.util.List; -import java.util.OptionalDouble; -import java.util.Queue; -import java.util.concurrent.ArrayBlockingQueue; -import java.util.function.Supplier; -import org.littletonrobotics.junction.Logger; - -/** - * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. - * - *

This version is intended for devices like the SparkMax that require polling rather than a - * blocking thread. A Notifier thread is used to gather samples with consistent timing. - */ -public class SparkMaxOdometryThread { - private List> signals = new ArrayList<>(); - private List> queues = new ArrayList<>(); - private List> timestampQueues = new ArrayList<>(); - - private final Notifier notifier; - private static SparkMaxOdometryThread instance = null; - - public static SparkMaxOdometryThread getInstance() { - if (instance == null) { - instance = new SparkMaxOdometryThread(); - } - return instance; - } - - private SparkMaxOdometryThread() { - notifier = new Notifier(this::periodic); - notifier.setName("SparkMaxOdometryThread"); - } - - public void start() { - if (timestampQueues.size() > 0) { - notifier.startPeriodic(1.0 / Module.ODOMETRY_FREQUENCY); - } - } - - public Queue registerSignal(Supplier signal) { - Queue queue = new ArrayBlockingQueue<>(20); - Drive.odometryLock.lock(); - try { - signals.add(signal); - queues.add(queue); - } finally { - Drive.odometryLock.unlock(); - } - return queue; - } - - public Queue makeTimestampQueue() { - Queue queue = new ArrayBlockingQueue<>(20); - Drive.odometryLock.lock(); - try { - timestampQueues.add(queue); - } finally { - Drive.odometryLock.unlock(); - } - return queue; - } - - private void periodic() { - Drive.odometryLock.lock(); - double timestamp = Logger.getRealTimestamp() / 1e6; - try { - double[] values = new double[signals.size()]; - boolean isValid = true; - for (int i = 0; i < signals.size(); i++) { - OptionalDouble value = signals.get(i).get(); - if (value.isPresent()) { - values[i] = value.getAsDouble(); - } else { - isValid = false; - break; - } - } - if (isValid) { - for (int i = 0; i < queues.size(); i++) { - queues.get(i).offer(values[i]); - } - for (int i = 0; i < timestampQueues.size(); i++) { - timestampQueues.get(i).offer(timestamp); - } - } - } finally { - Drive.odometryLock.unlock(); - } - } -} From ff5774de0d5b27675a31db73ca839763a4c6677a Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 2 Oct 2024 12:56:05 -0700 Subject: [PATCH 09/57] Adding in YAGSL config files new file: src/main/deploy/swerve/example.txt new file: src/main/deploy/swerve/falcon/controllerproperties.json new file: src/main/deploy/swerve/falcon/modules/backleft.json new file: src/main/deploy/swerve/falcon/modules/backright.json new file: src/main/deploy/swerve/falcon/modules/frontleft.json new file: src/main/deploy/swerve/falcon/modules/frontright.json new file: src/main/deploy/swerve/falcon/modules/pdifproperties.json new file: src/main/deploy/swerve/falcon/modules/physicalproperties.json new file: src/main/deploy/swerve/falcon/swervedrive.json modified: src/main/java/frc/robot/Main.java modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/util/OverrideSwitches.java --- src/main/deploy/swerve/example.txt | 3 ++ .../swerve/falcon/controllerproperties.json | 8 ++++ .../swerve/falcon/modules/backleft.json | 26 +++++++++++++ .../swerve/falcon/modules/backright.json | 26 +++++++++++++ .../swerve/falcon/modules/frontleft.json | 26 +++++++++++++ .../swerve/falcon/modules/frontright.json | 26 +++++++++++++ .../swerve/falcon/modules/pdifproperties.json | 16 ++++++++ .../falcon/modules/physicalproperties.json | 16 ++++++++ .../deploy/swerve/falcon/swervedrive.json | 14 +++++++ src/main/java/frc/robot/Main.java | 4 +- src/main/java/frc/robot/Robot.java | 38 +++++++++---------- src/main/java/frc/robot/RobotContainer.java | 29 ++++++++++---- .../swervedrive/SwerveSubsystem.java | 2 + .../java/frc/robot/util/OverrideSwitches.java | 9 +++++ 14 files changed, 214 insertions(+), 29 deletions(-) create mode 100644 src/main/deploy/swerve/example.txt create mode 100644 src/main/deploy/swerve/falcon/controllerproperties.json create mode 100644 src/main/deploy/swerve/falcon/modules/backleft.json create mode 100644 src/main/deploy/swerve/falcon/modules/backright.json create mode 100644 src/main/deploy/swerve/falcon/modules/frontleft.json create mode 100644 src/main/deploy/swerve/falcon/modules/frontright.json create mode 100644 src/main/deploy/swerve/falcon/modules/pdifproperties.json create mode 100644 src/main/deploy/swerve/falcon/modules/physicalproperties.json create mode 100644 src/main/deploy/swerve/falcon/swervedrive.json diff --git a/src/main/deploy/swerve/example.txt b/src/main/deploy/swerve/example.txt new file mode 100644 index 00000000..bb82515d --- /dev/null +++ b/src/main/deploy/swerve/example.txt @@ -0,0 +1,3 @@ +Files placed in this directory will be deployed to the RoboRIO into the +'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function +to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/src/main/deploy/swerve/falcon/controllerproperties.json b/src/main/deploy/swerve/falcon/controllerproperties.json new file mode 100644 index 00000000..95a19ac8 --- /dev/null +++ b/src/main/deploy/swerve/falcon/controllerproperties.json @@ -0,0 +1,8 @@ +{ + "angleJoystickRadiusDeadband": 0.5, + "heading": { + "p": 0.118, + "i": 0, + "d": 0 + } +} diff --git a/src/main/deploy/swerve/falcon/modules/backleft.json b/src/main/deploy/swerve/falcon/modules/backleft.json new file mode 100644 index 00000000..fd39bc0d --- /dev/null +++ b/src/main/deploy/swerve/falcon/modules/backleft.json @@ -0,0 +1,26 @@ +{ + "drive": { + "type": "talonfx", + "id": 4, + "canbus": null + }, + "angle": { + "type": "talonfx", + "id": 2, + "canbus": null + }, + "encoder": { + "type": "cancoder", + "id": 1, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + }, + "absoluteEncoderOffset": -259.805, + "location": { + "front": -7, + "left": 7 + } +} diff --git a/src/main/deploy/swerve/falcon/modules/backright.json b/src/main/deploy/swerve/falcon/modules/backright.json new file mode 100644 index 00000000..11ac2b94 --- /dev/null +++ b/src/main/deploy/swerve/falcon/modules/backright.json @@ -0,0 +1,26 @@ +{ + "drive": { + "type": "talonfx", + "id": 3, + "canbus": null + }, + "angle": { + "type": "talonfx", + "id": 53, + "canbus": null + }, + "encoder": { + "type": "cancoder", + "id": 4, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + }, + "absoluteEncoderOffset": -31.816, + "location": { + "front": -7, + "left": -7 + } +} diff --git a/src/main/deploy/swerve/falcon/modules/frontleft.json b/src/main/deploy/swerve/falcon/modules/frontleft.json new file mode 100644 index 00000000..c6abe46e --- /dev/null +++ b/src/main/deploy/swerve/falcon/modules/frontleft.json @@ -0,0 +1,26 @@ +{ + "drive": { + "type": "talonfx", + "id": 50, + "canbus": null + }, + "angle": { + "type": "talonfx", + "id": 8, + "canbus": null + }, + "encoder": { + "type": "cancoder", + "id": 3, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + }, + "absoluteEncoderOffset": -139.043, + "location": { + "front": 7, + "left": 7 + } +} diff --git a/src/main/deploy/swerve/falcon/modules/frontright.json b/src/main/deploy/swerve/falcon/modules/frontright.json new file mode 100644 index 00000000..ba24422c --- /dev/null +++ b/src/main/deploy/swerve/falcon/modules/frontright.json @@ -0,0 +1,26 @@ +{ + "drive": { + "type": "talonfx", + "id": 51, + "canbus": null + }, + "angle": { + "type": "talonfx", + "id": 52, + "canbus": null + }, + "encoder": { + "type": "cancoder", + "id": 2, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + }, + "absoluteEncoderOffset": -118.564, + "location": { + "front": 7, + "left": -7 + } +} diff --git a/src/main/deploy/swerve/falcon/modules/pdifproperties.json b/src/main/deploy/swerve/falcon/modules/pdifproperties.json new file mode 100644 index 00000000..75d55289 --- /dev/null +++ b/src/main/deploy/swerve/falcon/modules/pdifproperties.json @@ -0,0 +1,16 @@ +{ + "drive": { + "p": 1, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + }, + "angle": { + "p": 50, + "i": 0, + "d": 0.32, + "f": 0, + "iz": 0 + } +} diff --git a/src/main/deploy/swerve/falcon/modules/physicalproperties.json b/src/main/deploy/swerve/falcon/modules/physicalproperties.json new file mode 100644 index 00000000..a0a7b127 --- /dev/null +++ b/src/main/deploy/swerve/falcon/modules/physicalproperties.json @@ -0,0 +1,16 @@ +{ + "conversionFactor": { + "angle": 28.125, + "drive": 0.047286787200699704 + }, + "currentLimit": { + "drive": 40, + "angle": 20 + }, + "rampRate": { + "drive": 0.25, + "angle": 0.25 + }, + "wheelGripCoefficientOfFriction": 1.19, + "optimalVoltage": 12 +} diff --git a/src/main/deploy/swerve/falcon/swervedrive.json b/src/main/deploy/swerve/falcon/swervedrive.json new file mode 100644 index 00000000..509063d9 --- /dev/null +++ b/src/main/deploy/swerve/falcon/swervedrive.json @@ -0,0 +1,14 @@ +{ + "imu": { + "type": "navx", + "id": 13, + "canbus": "canivore" + }, + "invertedIMU": true, + "modules": [ + "frontleft.json", + "frontright.json", + "backleft.json", + "backright.json" + ] +} diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index e8af1c10..750b769a 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -1,5 +1,5 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1a1ee3d5..6302d85a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -38,9 +38,9 @@ * project. */ public class Robot extends LoggedRobot { - private Command autonomousCommand; - private RobotContainer robotContainer; - private Timer disabledTimer; + private Command m_autonomousCommand; + private RobotContainer m_robotContainer; + private Timer m_disabledTimer; /** * This function is run when the robot is first started up and should be used for any @@ -99,11 +99,11 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. - robotContainer = new RobotContainer(); + m_robotContainer = new RobotContainer(); // Create a timer to disable motor brake a few seconds after disable. This will let the robot // stop immediately when disabled, but then also let it be pushed more - disabledTimer = new Timer(); + m_disabledTimer = new Timer(); } /** This function is called periodically during all modes. */ @@ -124,30 +124,30 @@ public void robotPeriodic() { @Override public void disabledInit() { // Set the brakes to stop robot motion - robotContainer.setMotorBrake(true); - disabledTimer.reset(); - disabledTimer.start(); + m_robotContainer.setMotorBrake(true); + m_disabledTimer.reset(); + m_disabledTimer.start(); } /** This function is called periodically when disabled. */ @Override public void disabledPeriodic() { // After WHEEL_LOCK_TIME has elapsed, release the drive brakes - if (disabledTimer.hasElapsed(Constants.DrivebaseConstants.WHEEL_LOCK_TIME)) { - robotContainer.setMotorBrake(false); - disabledTimer.stop(); + if (m_disabledTimer.hasElapsed(Constants.DrivebaseConstants.WHEEL_LOCK_TIME)) { + m_robotContainer.setMotorBrake(false); + m_disabledTimer.stop(); } } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { - robotContainer.setMotorBrake(true); - autonomousCommand = robotContainer.getAutonomousCommand(); + m_robotContainer.setMotorBrake(true); + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) - if (autonomousCommand != null) { - autonomousCommand.schedule(); + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); } } @@ -162,11 +162,11 @@ public void teleopInit() { // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - if (autonomousCommand != null) { - autonomousCommand.cancel(); + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); } - robotContainer.setDriveMode(); - robotContainer.setMotorBrake(true); + m_robotContainer.setDriveMode(); + m_robotContainer.setMotorBrake(true); } /** This function is called periodically during operator control. */ @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 81abfe99..c5ee9eb0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,3 +1,16 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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. +// // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. @@ -20,7 +33,7 @@ public class RobotContainer { // Replace with CommandPS4Controller or CommandJoystick if needed final CommandXboxController driverXbox = new CommandXboxController(0); // The robot's subsystems and commands are defined here... - private final SwerveSubsystem drivebase = + private final SwerveSubsystem m_drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo")); /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -38,7 +51,7 @@ public RobotContainer() { // WARNING: default buttons are on the same buttons as the ones defined in configureBindings AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv( - drivebase, + m_drivebase, () -> -MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), () -> -MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), () -> @@ -54,7 +67,7 @@ public RobotContainer() { // left stick controls translation // right stick controls the desired angle NOT angular rotation Command driveFieldOrientedDirectAngle = - drivebase.driveCommand( + m_drivebase.driveCommand( () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), () -> driverXbox.getRightX(), @@ -66,18 +79,18 @@ public RobotContainer() { // left stick controls translation // right stick controls the angular velocity of the robot Command driveFieldOrientedAnglularVelocity = - drivebase.driveCommand( + m_drivebase.driveCommand( () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), () -> driverXbox.getRightX() * 0.5); Command driveFieldOrientedDirectAngleSim = - drivebase.simDriveCommand( + m_drivebase.simDriveCommand( () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), () -> driverXbox.getRawAxis(2)); - drivebase.setDefaultCommand( + m_drivebase.setDefaultCommand( !RobotBase.isSimulation() ? driveFieldOrientedDirectAngle : driveFieldOrientedDirectAngleSim); @@ -101,7 +114,7 @@ private void configureBindings() {} */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return drivebase.getAutonomousCommand("New Auto"); + return m_drivebase.getAutonomousCommand("New Auto"); } public void setDriveMode() { @@ -109,6 +122,6 @@ public void setDriveMode() { } public void setMotorBrake(boolean brake) { - drivebase.setMotorBrake(brake); + m_drivebase.setMotorBrake(brake); } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 37f5da40..c6ed94fb 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -64,9 +64,11 @@ public class SwerveSubsystem extends SubsystemBase { private Vision vision; /** Swerve drive object. */ private final SwerveDrive swerveDrive; + /** AprilTag field layout. */ private final AprilTagFieldLayout aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + /** Enable vision odometry updates while driving. */ private final boolean visionDriveTest = false; diff --git a/src/main/java/frc/robot/util/OverrideSwitches.java b/src/main/java/frc/robot/util/OverrideSwitches.java index 05576bed..a04b00e3 100644 --- a/src/main/java/frc/robot/util/OverrideSwitches.java +++ b/src/main/java/frc/robot/util/OverrideSwitches.java @@ -13,6 +13,15 @@ // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. +// NOTE: This module assumes the design described in FRC6328's 2024 Chief +// Delphi build thread: +// https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2024-build-thread/442736/72 +// +// The "Arcade Controller" used by 6328 (https://www.amazon.com/gp/product/B00UUROWWK) +// enumerates as a “Generic USB Controller”, mapped in this modules as a GenericHID. +// +// If another type of controller is used, swap in the proper class. + package frc.robot.util; import edu.wpi.first.wpilibj.DriverStation; From 7c113941a5599d1ed2cdedcdf5f4ccfa730b8e51 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 9 Oct 2024 12:26:19 -0700 Subject: [PATCH 10/57] Moving toward Power Monitoring and Management Starting to outline power management with monitoring, Still need to think about how to prioritize current to certain motors (and a wrapper for pushing current maximums) when total current draw exceeds the limits. Right now, logging and alerts are used to warn the operator if over budget on amps. modified: .vscode/settings.json modified: src/main/java/frc/robot/Constants.java new file: src/main/java/frc/robot/Ports.java modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/RobotContainer.java new file: src/main/java/frc/robot/util/CanDeviceId.java new file: src/main/java/frc/robot/util/PowerMonitoring.java --- .vscode/settings.json | 3 +- src/main/java/frc/robot/Constants.java | 22 +++++- src/main/java/frc/robot/Ports.java | 66 ++++++++++++++++ src/main/java/frc/robot/Robot.java | 7 +- src/main/java/frc/robot/RobotContainer.java | 3 + src/main/java/frc/robot/util/CanDeviceId.java | 47 +++++++++++ .../java/frc/robot/util/PowerMonitoring.java | 77 +++++++++++++++++++ 7 files changed, 221 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/Ports.java create mode 100644 src/main/java/frc/robot/util/CanDeviceId.java create mode 100644 src/main/java/frc/robot/util/PowerMonitoring.java diff --git a/.vscode/settings.json b/.vscode/settings.json index a2e5322d..4b971ad8 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -28,5 +28,6 @@ }, null ], - "java.test.defaultConfig": "WPIlibUnitTests" + "java.test.defaultConfig": "WPIlibUnitTests", + "editor.indentSize": 2 } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0649f357..e6faf4ff 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,7 +1,5 @@ // Copyright (c) 2024 Az-FIRST // http://github.com/AZ-First -// Copyright (c) 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License @@ -22,6 +20,7 @@ import com.pathplanner.lib.util.PIDConstants; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.RobotBase; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; @@ -42,6 +41,25 @@ public final class Constants { private static RobotType robotType = RobotType.COMPBOT; public static final boolean tuningMode = false; + /* Power Distribution Module Constants */ + public static final class PowerDistributionConstants { + + // Set this to either kRev or kCTRE for the type of Power Distribution Module + public static final ModuleType kPowerModule = ModuleType.kRev; + + public static final double kTotalMaxCurrent = 120.; + public static final double kMotorPortMaxCurrent = 40.; + public static final double kSmallPortMaxCurrent = 20.; + + // The Power Distribution Module ports into which the DRIVE motors are plugged + public static final int[] kDrivePowerPorts = {0, 2, 4, 6}; + // The Power Distribution Module ports into which the STEER motors are plugged + public static final int[] kSteerPowerPorts = {1, 3, 5, 7}; + // Add additional subsystem port enumerations here for combined monitoring + // Example: + // public static final int[] kElevatorPowerPorts = {9, 10}; + } + /* Physical Constants for Robot Operation */ public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound public static final Matter CHASSIS = diff --git a/src/main/java/frc/robot/Ports.java b/src/main/java/frc/robot/Ports.java new file mode 100644 index 00000000..0511d5cb --- /dev/null +++ b/src/main/java/frc/robot/Ports.java @@ -0,0 +1,66 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright (c) 2024 FRC 1678 +// https://github.com/frc1678 +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot; + +import frc.robot.util.CanDeviceId; + +/** List of Channel and CAN IDs */ +public class Ports { + + /* DRIVETRAIN CAN DEVICE IDS */ + // This is the default setup for the Az-RBSI swerve base + // Swerve Modules go: + // FL,FR,BL,BR + // + // 0 1 + // 2 3 + public static final CanDeviceId FL_DRIVE = new CanDeviceId(1, "canivore"); + public static final CanDeviceId FL_ROTATION = new CanDeviceId(2, "canivore"); + public static final CanDeviceId FL_CANCODER = new CanDeviceId(3, "canivore"); + + public static final CanDeviceId FR_DRIVE = new CanDeviceId(4, "canivore"); + public static final CanDeviceId FR_ROTATION = new CanDeviceId(5, "canivore"); + public static final CanDeviceId FR_CANCODER = new CanDeviceId(6, "canivore"); + + public static final CanDeviceId BL_DRIVE = new CanDeviceId(7, "canivore"); + public static final CanDeviceId BL_ROTATION = new CanDeviceId(8, "canivore"); + public static final CanDeviceId BL_CANCODER = new CanDeviceId(9, "canivore"); + + public static final CanDeviceId BR_DRIVE = new CanDeviceId(10, "canivore"); + public static final CanDeviceId BR_ROTATION = new CanDeviceId(11, "canivore"); + public static final CanDeviceId BR_CANCODER = new CanDeviceId(12, "canivore"); + + public static final CanDeviceId PIGEON = new CanDeviceId(13, "canivore"); + + /* POWER DISTRIBUTION CAN ID */ + public static final CanDeviceId POWER_CAN_DEVICE_ID = new CanDeviceId(1, ""); + + /* SUBSYSTEM CAN DEVICE IDS */ + // This is where mechanism subsystem devices are defined + // Example: + // public static final CanDeviceId ELEVATOR_MAIN = new CanDeviceId(3, ""); + + /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */ + // This is where digital I/O feedback devices are defined + // Example: + // public static final int ELEVATOR_BOTTOM_LIMIT = 3; + + /* LINEAR SERVO PWM CHANNELS */ + // This is where PWM-controlled devices (actuators, servos, pneumatics, etc.) + // are defined + // Example: + // public static final int INTAKE_SERVO = 0; +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6302d85a..9c3fcd82 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -26,6 +26,7 @@ import org.littletonrobotics.junction.LogTable; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggedPowerDistribution; import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; @@ -72,6 +73,8 @@ public void robotInit() { // Running on a real robot, log to a USB stick ("/U/logs") Logger.addDataReceiver(new WPILOGWriter()); Logger.addDataReceiver(new NT4Publisher()); + LoggedPowerDistribution.getInstance( + Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), Constants.kPowerModule); break; case SIM: @@ -101,7 +104,8 @@ public void robotInit() { // and put our autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - // Create a timer to disable motor brake a few seconds after disable. This will let the robot + // Create a timer to disable motor brake a few seconds after disable. This will + // let the robot // stop immediately when disabled, but then also let it be pushed more m_disabledTimer = new Timer(); } @@ -168,6 +172,7 @@ public void teleopInit() { m_robotContainer.setDriveMode(); m_robotContainer.setMotorBrake(true); } + /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c5ee9eb0..3cf66ce8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,8 +30,11 @@ public class RobotContainer { + // Define the Driver and, optionally, the Operator/Co-Driver Controllers // Replace with CommandPS4Controller or CommandJoystick if needed final CommandXboxController driverXbox = new CommandXboxController(0); + final CommandXboxController operatorXbox = new CommandXboxController(1); + // The robot's subsystems and commands are defined here... private final SwerveSubsystem m_drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo")); diff --git a/src/main/java/frc/robot/util/CanDeviceId.java b/src/main/java/frc/robot/util/CanDeviceId.java new file mode 100644 index 00000000..6421843d --- /dev/null +++ b/src/main/java/frc/robot/util/CanDeviceId.java @@ -0,0 +1,47 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright (c) 2024 FRC 254 +// https://github.com/team254 +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util; + +/** Class for wrapping CAN devices with a name and functionality */ +public class CanDeviceId { + private final int mDeviceNumber; + private final String mBus; + + public CanDeviceId(int deviceNumber, String bus) { + mDeviceNumber = deviceNumber; + mBus = bus; + } + + /** Use the default bus name (empty string) */ + public CanDeviceId(int deviceNumber) { + this(deviceNumber, ""); + } + + /** Get the CAN ID value for a named device */ + public int getDeviceNumber() { + return mDeviceNumber; + } + + /** Get the CAN bus name for a named device */ + public String getBus() { + return mBus; + } + + /** Check whether two named devices are, in fact, the same */ + public boolean equals(CanDeviceId other) { + return other.mDeviceNumber == mDeviceNumber && other.mBus == mBus; + } +} diff --git a/src/main/java/frc/robot/util/PowerMonitoring.java b/src/main/java/frc/robot/util/PowerMonitoring.java new file mode 100644 index 00000000..39247764 --- /dev/null +++ b/src/main/java/frc/robot/util/PowerMonitoring.java @@ -0,0 +1,77 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util; + +import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants.PowerDistributionConstants; +import frc.robot.Ports; +import frc.robot.util.Alert.AlertType; +import org.littletonrobotics.junction.Logger; + +public class PowerMonitoring extends VirtualSubsystem { + + private PowerDistribution powerDistributionModule = + new PowerDistribution( + Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerDistributionConstants.kPowerModule); + private int NUM_PDH_CHANNELS = powerDistributionModule.getNumChannels(); + private double[] channelCurrents = new double[NUM_PDH_CHANNELS]; + + /** Periodic Method */ + public void periodic() { + + // Check the total robot current and individual port currents against Constants + double totalCurrent = powerDistributionModule.getTotalCurrent(); + if (totalCurrent > PowerDistributionConstants.kTotalMaxCurrent) { + new Alert("Total current draw exceeds limit!", AlertType.WARNING).set(true); + } + for (int i = 0; i < NUM_PDH_CHANNELS; i++) { + channelCurrents[i] = powerDistributionModule.getCurrent(i); + if (channelCurrents[i] > PowerDistributionConstants.kMotorPortMaxCurrent) { + new Alert("Port " + i + " current draw exceeds limit!", AlertType.WARNING).set(true); + } + } + + // Compute DRIVE and STEER summed current + double driveCurrent = 0.0; + double steerCurrent = 0.0; + for (int port : PowerDistributionConstants.kDrivePowerPorts) { + driveCurrent += channelCurrents[port]; + } + for (int port : PowerDistributionConstants.kSteerPowerPorts) { + steerCurrent += channelCurrents[port]; + } + // Add current monitoring by subsystem here + // Example: + // double elevatorCurrent = 0.0; + // for (int port : PowerDistributionConstants.kElevatorPowerPorts) { + // elevatorCurrent += channelCurrents[port]; + // } + + // Log values to AdvantageKit and to SmartDashboard + Logger.recordOutput("PowerMonitor/TotalCurrent", totalCurrent); + Logger.recordOutput("PowerMonitor/DriveCurrent", driveCurrent); + Logger.recordOutput("PowerMonitor/SteerCurrent", steerCurrent); + SmartDashboard.putNumber("TotalCurrent", totalCurrent); + SmartDashboard.putNumber("DriveCurrent", driveCurrent); + SmartDashboard.putNumber("SteerCurrent", steerCurrent); + // Add logging for subsystems here + // Example: + // Logger.recordOutput("PowerMonitor/ElevatorCurrent", elevatorCurrent); + // SmartDashboard.putNumber("ElevatorCurrent", elevatorCurrent); + + // Do something about setting priorities if drawing too much current + + } +} From 60777b30fac94cdd9ab2f83c86c2d5aa5f71ab2d Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 9 Oct 2024 12:40:45 -0700 Subject: [PATCH 11/57] Add accelerometer VirtualSubsystem to report accels Log and report accelerations from both the on-board RIO accelerometer and the Pigeon2 that is part of the RB drivebase. modified: src/main/java/frc/robot/Robot.java new file: src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java new file: src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIO.java new file: src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIOPigeon2.java new file: src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIORio.java --- src/main/java/frc/robot/Robot.java | 3 +- .../accelerometer/Accelerometer.java | 39 ++++++++++++++++ .../accelerometer/AccelerometerIO.java | 46 +++++++++++++++++++ .../accelerometer/AccelerometerIOPigeon2.java | 42 +++++++++++++++++ .../accelerometer/AccelerometerIORio.java | 16 +++++++ 5 files changed, 145 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java create mode 100644 src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIO.java create mode 100644 src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIOPigeon2.java create mode 100644 src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIORio.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9c3fcd82..a43403a0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.Constants.PowerDistributionConstants; import frc.robot.util.VirtualSubsystem; import java.io.File; import java.io.IOException; @@ -74,7 +75,7 @@ public void robotInit() { Logger.addDataReceiver(new WPILOGWriter()); Logger.addDataReceiver(new NT4Publisher()); LoggedPowerDistribution.getInstance( - Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), Constants.kPowerModule); + Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerDistributionConstants.kPowerModule); break; case SIM: diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java new file mode 100644 index 00000000..5c06625a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -0,0 +1,39 @@ +// Copyright (c) 2024 FRC 2486 +// http://github.com/Coconuts2486-FRC +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.accelerometer; + +import frc.robot.subsystems.accelerometer.AccelerometerIO.AccelerometerIOInputs; +import frc.robot.util.VirtualSubsystem; + +/** Accelerometer subsystem (built upon a virtual subsystem) */ +public class Accelerometer extends VirtualSubsystem { + + private final AccelerometerIO[] io; + private final AccelerometerIOInputs[] inputs; + + // Class method definition, including inputs + public Accelerometer(AccelerometerIO... io) { + this.io = io; + inputs = new AccelerometerIOInputs[io.length]; + for (int i = 0; i < io.length; i++) { + inputs[i] = new AccelerometerIOInputs(); + } + } + + public void periodic() { + for (int i = 0; i < io.length; i++) { + io[i].updateInputs(inputs[i]); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIO.java b/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIO.java new file mode 100644 index 00000000..8e1f79ce --- /dev/null +++ b/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIO.java @@ -0,0 +1,46 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// Copyright (c) 2024 FRC 2486 +// http://github.com/Coconuts2486-FRC +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.accelerometer; + +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +public interface AccelerometerIO { + @AutoLog + class AccelerometerIOInputs implements LoggableInputs { + public boolean connected = false; + public double accelXVal = 0.0; + public double accelYVal = 0.0; + public double accelZVal = 0.0; + + @Override + public void toLog(LogTable table) { + table.put("AccelX", accelXVal); + table.put("AccelY", accelYVal); + table.put("AccelZ", accelZVal); + } + + @Override + public void fromLog(LogTable table) { + accelXVal = table.get("AccelX", 0.0); + accelYVal = table.get("AccelY", 0.0); + accelZVal = table.get("AccelZ", 0.0); + } + } + + default void updateInputs(AccelerometerIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIOPigeon2.java b/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIOPigeon2.java new file mode 100644 index 00000000..e4ed0dd3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIOPigeon2.java @@ -0,0 +1,42 @@ +// Copyright (c) 2024 FRC 2486 +// http://github.com/Coconuts2486-FRC +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.accelerometer; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.Pigeon2; + +public class AccelerometerIOPigeon2 implements AccelerometerIO { + + public final String accelName = "Pigeon2"; + private final Pigeon2 pigeon = new Pigeon2(20); + private final StatusSignal accelX = pigeon.getAccelerationX(); + private final StatusSignal accelY = pigeon.getAccelerationY(); + private final StatusSignal accelZ = pigeon.getAccelerationZ(); + + public AccelerometerIOPigeon2() { + accelX.setUpdateFrequency(100.0); + accelY.setUpdateFrequency(100.0); + accelZ.setUpdateFrequency(100.0); + pigeon.optimizeBusUtilization(); + } + + public void updateInputs(AccelerometerIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(accelX, accelY, accelZ).equals(StatusCode.OK); + inputs.accelXVal = accelX.getValueAsDouble(); + inputs.accelYVal = accelY.getValueAsDouble(); + inputs.accelZVal = accelZ.getValueAsDouble(); + } +} diff --git a/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIORio.java b/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIORio.java new file mode 100644 index 00000000..81cf4559 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIORio.java @@ -0,0 +1,16 @@ +// Copyright (c) 2024 FRC 2486 +// http://github.com/Coconuts2486-FRC +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.accelerometer; + +public class AccelerometerIORio {} From 69952d3665e155109c929bac176437d154ed3a8c Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 9 Oct 2024 13:04:43 -0700 Subject: [PATCH 12/57] Cleaning up constants a bit modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/util/PowerMonitoring.java --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/Robot.java | 9 +++----- src/main/java/frc/robot/RobotContainer.java | 22 +++++++++---------- .../java/frc/robot/util/PowerMonitoring.java | 14 ++++++------ 4 files changed, 22 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e6faf4ff..0006dce7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -47,6 +47,7 @@ public static final class PowerDistributionConstants { // Set this to either kRev or kCTRE for the type of Power Distribution Module public static final ModuleType kPowerModule = ModuleType.kRev; + // Current Limits public static final double kTotalMaxCurrent = 120.; public static final double kMotorPortMaxCurrent = 40.; public static final double kSmallPortMaxCurrent = 20.; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a43403a0..65d7e9b4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,7 +1,5 @@ // Copyright (c) 2024 Az-FIRST // http://github.com/AZ-First -// Copyright (c) 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License @@ -15,11 +13,12 @@ package frc.robot; +import static frc.robot.Constants.PowerDistributionConstants.*; + import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.Constants.PowerDistributionConstants; import frc.robot.util.VirtualSubsystem; import java.io.File; import java.io.IOException; @@ -75,7 +74,7 @@ public void robotInit() { Logger.addDataReceiver(new WPILOGWriter()); Logger.addDataReceiver(new NT4Publisher()); LoggedPowerDistribution.getInstance( - Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerDistributionConstants.kPowerModule); + Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), kPowerModule); break; case SIM: @@ -99,8 +98,6 @@ public void robotInit() { Logger.start(); LogTable.disableProtobufWarning(); - Logger.recordOutput("Cmd_Status/GP Tracking", false); - // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3cf66ce8..5dd7f3f1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,13 +17,14 @@ package frc.robot; +import static frc.robot.Constants.OperatorConstants.*; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.OperatorConstants; import frc.robot.commands.swervedrive.drivebase.AbsoluteDriveAdv; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.io.File; @@ -55,10 +56,9 @@ public RobotContainer() { AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv( m_drivebase, - () -> -MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), - () -> -MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> - -MathUtil.applyDeadband(driverXbox.getRightX(), OperatorConstants.RIGHT_X_DEADBAND), + () -> -MathUtil.applyDeadband(driverXbox.getLeftY(), LEFT_Y_DEADBAND), + () -> -MathUtil.applyDeadband(driverXbox.getLeftX(), LEFT_X_DEADBAND), + () -> -MathUtil.applyDeadband(driverXbox.getRightX(), RIGHT_X_DEADBAND), driverXbox.getHID()::getYButtonPressed, driverXbox.getHID()::getAButtonPressed, driverXbox.getHID()::getXButtonPressed, @@ -71,8 +71,8 @@ public RobotContainer() { // right stick controls the desired angle NOT angular rotation Command driveFieldOrientedDirectAngle = m_drivebase.driveCommand( - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), LEFT_X_DEADBAND), () -> driverXbox.getRightX(), () -> driverXbox.getRightY()); @@ -83,14 +83,14 @@ public RobotContainer() { // right stick controls the angular velocity of the robot Command driveFieldOrientedAnglularVelocity = m_drivebase.driveCommand( - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), LEFT_X_DEADBAND), () -> driverXbox.getRightX() * 0.5); Command driveFieldOrientedDirectAngleSim = m_drivebase.simDriveCommand( - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), LEFT_X_DEADBAND), () -> driverXbox.getRawAxis(2)); m_drivebase.setDefaultCommand( diff --git a/src/main/java/frc/robot/util/PowerMonitoring.java b/src/main/java/frc/robot/util/PowerMonitoring.java index 39247764..8c119f06 100644 --- a/src/main/java/frc/robot/util/PowerMonitoring.java +++ b/src/main/java/frc/robot/util/PowerMonitoring.java @@ -13,9 +13,10 @@ package frc.robot.util; +import static frc.robot.Constants.PowerDistributionConstants.*; + import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants.PowerDistributionConstants; import frc.robot.Ports; import frc.robot.util.Alert.AlertType; import org.littletonrobotics.junction.Logger; @@ -23,8 +24,7 @@ public class PowerMonitoring extends VirtualSubsystem { private PowerDistribution powerDistributionModule = - new PowerDistribution( - Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerDistributionConstants.kPowerModule); + new PowerDistribution(Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), kPowerModule); private int NUM_PDH_CHANNELS = powerDistributionModule.getNumChannels(); private double[] channelCurrents = new double[NUM_PDH_CHANNELS]; @@ -33,12 +33,12 @@ public void periodic() { // Check the total robot current and individual port currents against Constants double totalCurrent = powerDistributionModule.getTotalCurrent(); - if (totalCurrent > PowerDistributionConstants.kTotalMaxCurrent) { + if (totalCurrent > kTotalMaxCurrent) { new Alert("Total current draw exceeds limit!", AlertType.WARNING).set(true); } for (int i = 0; i < NUM_PDH_CHANNELS; i++) { channelCurrents[i] = powerDistributionModule.getCurrent(i); - if (channelCurrents[i] > PowerDistributionConstants.kMotorPortMaxCurrent) { + if (channelCurrents[i] > kMotorPortMaxCurrent) { new Alert("Port " + i + " current draw exceeds limit!", AlertType.WARNING).set(true); } } @@ -46,10 +46,10 @@ public void periodic() { // Compute DRIVE and STEER summed current double driveCurrent = 0.0; double steerCurrent = 0.0; - for (int port : PowerDistributionConstants.kDrivePowerPorts) { + for (int port : kDrivePowerPorts) { driveCurrent += channelCurrents[port]; } - for (int port : PowerDistributionConstants.kSteerPowerPorts) { + for (int port : kSteerPowerPorts) { steerCurrent += channelCurrents[port]; } // Add current monitoring by subsystem here From 15f7f73a6f9b998216452a7d8d8b6c21417fc355 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 10 Oct 2024 16:15:42 -0700 Subject: [PATCH 13/57] Organization and adding README's modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/FieldConstants2024.java new file: src/main/java/frc/robot/README modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java modified: src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java modified: src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java modified: src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java modified: src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIO.java modified: src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIOPigeon2.java modified: src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIORio.java modified: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java modified: src/main/java/frc/robot/util/PowerMonitoring.java new file: src/test/README new file: src/test/RobotContainerTest.java --- src/main/java/frc/robot/Constants.java | 131 +++++++++--------- .../java/frc/robot/FieldConstants2024.java | 4 +- src/main/java/frc/robot/README | 73 ++++++++++ src/main/java/frc/robot/Robot.java | 8 +- src/main/java/frc/robot/RobotContainer.java | 22 +-- .../swervedrive/drivebase/AbsoluteDrive.java | 8 +- .../drivebase/AbsoluteDriveAdv.java | 13 +- .../drivebase/AbsoluteFieldDrive.java | 8 +- .../accelerometer/Accelerometer.java | 4 +- .../accelerometer/AccelerometerIO.java | 6 +- .../accelerometer/AccelerometerIOPigeon2.java | 4 +- .../accelerometer/AccelerometerIORio.java | 4 +- .../swervedrive/SwerveSubsystem.java | 10 +- .../java/frc/robot/util/PowerMonitoring.java | 14 +- src/test/README | 1 + src/test/RobotContainerTest.java | 26 ++++ 16 files changed, 219 insertions(+), 117 deletions(-) create mode 100644 src/main/java/frc/robot/README create mode 100644 src/test/README create mode 100644 src/test/RobotContainerTest.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0006dce7..9dfbd790 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -36,12 +36,76 @@ */ public final class Constants { - /* General Constants */ + /***************************************************************************/ + /** + * Define the various multiple robots that use this same code (e.g., COMPBOT, DEVBOT, SIMBOT, + * etc.) and the operating modes of the code (REAL, SIM, or REPLAY) + */ + public static boolean disableHAL = false; + /** Enumerate the robot types (add additional bots here) */ + public static enum RobotType { + DEVBOT, // Development / Alpha / Practice Bot + COMPBOT, // Competition robot + SIMBOT // Simulated robot + } + /** Enumerate the robot operation modes */ + public static enum Mode { + REAL, // REAL == Running on a real robot + SIM, // SIM == Running a physics simulator + REPLAY // REPLAY == Replaying from a log file + } + + /** Get the current robot */ + public static RobotType getRobot() { + if (!disableHAL && RobotBase.isReal() && robotType == RobotType.SIMBOT) { + new Alert("Invalid robot selected, using competition robot as default.", AlertType.ERROR) + .set(true); + robotType = RobotType.COMPBOT; + } + return robotType; + } + + /** Get the current mode for robot operation, based on robot type */ + public static Mode getMode() { + return switch (robotType) { + case DEVBOT, COMPBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; + case SIMBOT -> Mode.SIM; + }; + } + + /** Disable the Hardware Abstraction Layer, if requested */ + public static void disableHAL() { + disableHAL = true; + } + + /** Checks whether the correct robot is selected when deploying. */ + public static void main(String... args) { + if (robotType == RobotType.SIMBOT) { + System.err.println("Cannot deploy, invalid robot selected: " + robotType); + System.exit(1); + } + } + + /***************************************************************************/ + /* The remainder of this file contains physical and/or software constants for the various subsystems of the robot */ + + /** General Constants */ + private static RobotType robotType = getRobot(); + public static final double loopPeriodSecs = 0.02; - private static RobotType robotType = RobotType.COMPBOT; public static final boolean tuningMode = false; - /* Power Distribution Module Constants */ + /** Physical Constants for Robot Operation */ + public static final class PhysicalConstants { + public static final double kRobotMass = (148 - 20.3) * 0.453592; // 32lbs * kg per pound + public static final Matter kChassis = + new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), PhysicalConstants.kRobotMass); + public static final double kLoopTime = 0.13; // s, 20ms + 110ms sprk max velocity lag + public static final double kMaxSpeed = Units.feetToMeters(14.5); + // Maximum speed of the robot in meters per second, used to limit acceleration. + } + + /** Power Distribution Module Constants */ public static final class PowerDistributionConstants { // Set this to either kRev or kCTRE for the type of Power Distribution Module @@ -61,17 +125,8 @@ public static final class PowerDistributionConstants { // public static final int[] kElevatorPowerPorts = {9, 10}; } - /* Physical Constants for Robot Operation */ - public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound - public static final Matter CHASSIS = - new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); - public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms sprk max velocity lag - public static final double MAX_SPEED = Units.feetToMeters(14.5); - // Maximum speed of the robot in meters per second, used to limit acceleration. - /** Autonomous Action Constants */ public static final class AutonConstants { - public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0); public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01); } @@ -92,56 +147,4 @@ public static class OperatorConstants { public static final double RIGHT_X_DEADBAND = 0.1; public static final double TURN_CONSTANT = 6; } - - /** Location to define multiple robots (e.g., COMPBOT, DEVBOT, SIMBOT, etc.) */ - public static RobotType getRobot() { - if (!disableHAL && RobotBase.isReal() && robotType == RobotType.SIMBOT) { - new Alert("Invalid robot selected, using competition robot as default.", AlertType.ERROR) - .set(true); - robotType = RobotType.COMPBOT; - } - return robotType; - } - - /** Get the current mode for robot operation, based on robot type */ - public static Mode getMode() { - return switch (robotType) { - case DEVBOT, COMPBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; - case SIMBOT -> Mode.SIM; - }; - } - - /** Enumerate the robot operation modes */ - public static enum Mode { - /** Running on a real robot. */ - REAL, - - /** Running a physics simulator. */ - SIM, - - /** Replaying from a log file. */ - REPLAY - } - - /** Enumerate the robot types (add additional bots here) */ - public enum RobotType { - SIMBOT, - DEVBOT, - COMPBOT - } - - /** Disable the Hardware Abstraction Layer, if needed */ - public static boolean disableHAL = false; - - public static void disableHAL() { - disableHAL = true; - } - - /** Checks whether the correct robot is selected when deploying. */ - public static void main(String... args) { - if (robotType == RobotType.SIMBOT) { - System.err.println("Cannot deploy, invalid robot selected: " + robotType); - System.exit(1); - } - } } diff --git a/src/main/java/frc/robot/FieldConstants2024.java b/src/main/java/frc/robot/FieldConstants2024.java index 2d12811b..a8fd80ce 100644 --- a/src/main/java/frc/robot/FieldConstants2024.java +++ b/src/main/java/frc/robot/FieldConstants2024.java @@ -1,7 +1,7 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage -// Copyright (c) 2024 FRC 2486 -// http://github.com/Coconuts2486-FRC // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License diff --git a/src/main/java/frc/robot/README b/src/main/java/frc/robot/README new file mode 100644 index 00000000..5abf1169 --- /dev/null +++ b/src/main/java/frc/robot/README @@ -0,0 +1,73 @@ +Az-RBSI -- Java Root Directory + +-------------------- +This directory contains the basic Java files needed to define and run an FRC +robot. The files that should exist in this directory are listed below, along +with a brief description of their contents and which files MUST be edited to +include information about the specific robot being controlled. + +BuildConstants.java + Not tracked by git, used by the build system to record information about + the latest build of the code. This file should NOT be altered, and will be + overwritten each time the code is built / compiled. + +Constants.java + A file for enumerating physical and software constants for the robot. The + main Constants class should contain subsystem-specific classes that list + the constants for each subsystem. This is useful for organization and + importing only the constants needed for a particular software module. This + file MUST be modified to include the proper robot constants in the code. + +FieldConstants2024.java + Dimensions of the field and 2024 ("Crescendo") specific field elements. + An updated field constants file will be made available each year following + FRC Kickoff. Teams should feel free to add specific field information to + this file they feel is useful for developing commands and/or autonomous + actions. + +Main.java + This is the file run by the RoboRIO virtual machine. Do NOT alter this + file. + +Ports.java + A file for enumerating the CANbus and DIO ports utilized by the various + devices on the robot. This file MUST be modified so that the subsystems + can find their proper devices. + +Robot.java + This file is called by Main.java and directs the operation of the robot + according to the currently commanded mode (disabled, autonomous, teleop, + test, and simulation). Care should be taken when modifying this module + to ensure proper operation of the robot. One section that would be useful + to modify is the test mode, where teams can implement pre-match systems + testing in a known sequence. + +RobotContainer.java + A file for holding robot subsystems and commands. This file is called + from Robot.java and is where all subsystems and commands should be gathered + and organized, including button bindings for operator controls. + + +-------------------- +Subdirectories: + +commands/ + Commands build in separate modules (rather than in subsystem definitions or + inline commands in RobotContainer.java) live in this directory. + Organization of files and subdirectories herein is left to the discrection + of teams. + +subsystems/ + The hardware definition of subsystems is done in this directory. + Organization of files and subdirectories herein is left to the discrection + of teams. + +util/ + Various utility functions used by the Az-RBSI are included in this + directory. Most teams will not need to modify these routines, but + additional utilities deemed necessary by individual teams may be placed + here for organizational purposes. + + +-------------------- +Last Modified: 10 Oct 2024, TPEB diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 65d7e9b4..acec29a9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,12 +13,11 @@ package frc.robot; -import static frc.robot.Constants.PowerDistributionConstants.*; - import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.Constants.PowerDistributionConstants; import frc.robot.util.VirtualSubsystem; import java.io.File; import java.io.IOException; @@ -50,6 +49,9 @@ public class Robot extends LoggedRobot { @Override public void robotInit() { // Record metadata + Logger.recordMetadata("Robot", Constants.getRobot().toString()); + Logger.recordMetadata("TuningMode", Boolean.toString(Constants.tuningMode)); + Logger.recordMetadata("RuntimeType", getRuntimeType().toString()); Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); @@ -74,7 +76,7 @@ public void robotInit() { Logger.addDataReceiver(new WPILOGWriter()); Logger.addDataReceiver(new NT4Publisher()); LoggedPowerDistribution.getInstance( - Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), kPowerModule); + Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerDistributionConstants.kPowerModule); break; case SIM: diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5dd7f3f1..3cf66ce8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,14 +17,13 @@ package frc.robot; -import static frc.robot.Constants.OperatorConstants.*; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.OperatorConstants; import frc.robot.commands.swervedrive.drivebase.AbsoluteDriveAdv; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.io.File; @@ -56,9 +55,10 @@ public RobotContainer() { AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv( m_drivebase, - () -> -MathUtil.applyDeadband(driverXbox.getLeftY(), LEFT_Y_DEADBAND), - () -> -MathUtil.applyDeadband(driverXbox.getLeftX(), LEFT_X_DEADBAND), - () -> -MathUtil.applyDeadband(driverXbox.getRightX(), RIGHT_X_DEADBAND), + () -> -MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), + () -> -MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), + () -> + -MathUtil.applyDeadband(driverXbox.getRightX(), OperatorConstants.RIGHT_X_DEADBAND), driverXbox.getHID()::getYButtonPressed, driverXbox.getHID()::getAButtonPressed, driverXbox.getHID()::getXButtonPressed, @@ -71,8 +71,8 @@ public RobotContainer() { // right stick controls the desired angle NOT angular rotation Command driveFieldOrientedDirectAngle = m_drivebase.driveCommand( - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), LEFT_X_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), () -> driverXbox.getRightX(), () -> driverXbox.getRightY()); @@ -83,14 +83,14 @@ public RobotContainer() { // right stick controls the angular velocity of the robot Command driveFieldOrientedAnglularVelocity = m_drivebase.driveCommand( - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), LEFT_X_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), () -> driverXbox.getRightX() * 0.5); Command driveFieldOrientedDirectAngleSim = m_drivebase.simDriveCommand( - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), LEFT_X_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), () -> driverXbox.getRawAxis(2)); m_drivebase.setDefaultCommand( diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java index 4cb7018b..53926f56 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java @@ -24,7 +24,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; +import frc.robot.Constants.PhysicalConstants; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.util.List; import java.util.function.DoubleSupplier; @@ -112,9 +112,9 @@ public void execute() { translation, swerve.getFieldVelocity(), swerve.getPose(), - Constants.LOOP_TIME, - Constants.ROBOT_MASS, - List.of(Constants.CHASSIS), + PhysicalConstants.kLoopTime, + PhysicalConstants.kRobotMass, + List.of(PhysicalConstants.kChassis), swerve.getSwerveDriveConfiguration()); SmartDashboard.putNumber("LimitedTranslation", translation.getX()); SmartDashboard.putString("Translation", translation.toString()); diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java index 33aff281..9c38ef2b 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java @@ -24,7 +24,8 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; +import frc.robot.Constants.OperatorConstants; +import frc.robot.Constants.PhysicalConstants; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.util.List; import java.util.function.BooleanSupplier; @@ -136,9 +137,9 @@ public void execute() { translation, swerve.getFieldVelocity(), swerve.getPose(), - Constants.LOOP_TIME, - Constants.ROBOT_MASS, - List.of(Constants.CHASSIS), + PhysicalConstants.kLoopTime, + PhysicalConstants.kRobotMass, + List.of(PhysicalConstants.kChassis), swerve.getSwerveDriveConfiguration()); SmartDashboard.putNumber("LimitedTranslation", translation.getX()); SmartDashboard.putString("Translation", translation.toString()); @@ -147,9 +148,7 @@ public void execute() { if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { resetHeading = true; swerve.drive( - translation, - (Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), - true); + translation, (OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), true); } else { swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java index 4b6d16fc..d939bd43 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java @@ -24,7 +24,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; +import frc.robot.Constants.PhysicalConstants; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.util.List; import java.util.function.DoubleSupplier; @@ -81,9 +81,9 @@ public void execute() { translation, swerve.getFieldVelocity(), swerve.getPose(), - Constants.LOOP_TIME, - Constants.ROBOT_MASS, - List.of(Constants.CHASSIS), + PhysicalConstants.kLoopTime, + PhysicalConstants.kRobotMass, + List.of(PhysicalConstants.kChassis), swerve.getSwerveDriveConfiguration()); SmartDashboard.putNumber("LimitedTranslation", translation.getX()); SmartDashboard.putString("Translation", translation.toString()); diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 5c06625a..9ab29557 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -1,5 +1,5 @@ -// Copyright (c) 2024 FRC 2486 -// http://github.com/Coconuts2486-FRC +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License diff --git a/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIO.java b/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIO.java index 8e1f79ce..f31132f7 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIO.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIO.java @@ -1,7 +1,5 @@ -// Copyright (c) 2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// Copyright (c) 2024 FRC 2486 -// http://github.com/Coconuts2486-FRC +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License diff --git a/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIOPigeon2.java b/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIOPigeon2.java index e4ed0dd3..f09d3299 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIOPigeon2.java @@ -1,5 +1,5 @@ -// Copyright (c) 2024 FRC 2486 -// http://github.com/Coconuts2486-FRC +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License diff --git a/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIORio.java b/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIORio.java index 81cf4559..b449d5b3 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIORio.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIORio.java @@ -1,5 +1,5 @@ -// Copyright (c) 2024 FRC 2486 -// http://github.com/Coconuts2486-FRC +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index c6ed94fb..1de53172 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -41,8 +41,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import frc.robot.Constants; import frc.robot.Constants.AutonConstants; +import frc.robot.Constants.PhysicalConstants; import java.io.File; import java.util.function.DoubleSupplier; import org.photonvision.PhotonCamera; @@ -99,7 +99,7 @@ public SwerveSubsystem(File directory) { // created. SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; try { - swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED); + swerveDrive = new SwerveParser(directory).createSwerveDrive(PhysicalConstants.kMaxSpeed); // Alternative method if you don't want to supply the conversion factor via JSON files. // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, // angleConversionFactor, driveConversionFactor); @@ -127,7 +127,7 @@ public SwerveSubsystem(File directory) { */ public SwerveSubsystem( SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { - swerveDrive = new SwerveDrive(driveCfg, controllerCfg, Constants.MAX_SPEED); + swerveDrive = new SwerveDrive(driveCfg, controllerCfg, PhysicalConstants.kMaxSpeed); } /** Setup the photon vision class. */ @@ -550,7 +550,7 @@ public ChassisSpeeds getTargetSpeeds( headingX, headingY, getHeading().getRadians(), - Constants.MAX_SPEED); + PhysicalConstants.kMaxSpeed); } /** @@ -570,7 +570,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d an scaledInputs.getY(), angle.getRadians(), getHeading().getRadians(), - Constants.MAX_SPEED); + PhysicalConstants.kMaxSpeed); } /** diff --git a/src/main/java/frc/robot/util/PowerMonitoring.java b/src/main/java/frc/robot/util/PowerMonitoring.java index 8c119f06..39247764 100644 --- a/src/main/java/frc/robot/util/PowerMonitoring.java +++ b/src/main/java/frc/robot/util/PowerMonitoring.java @@ -13,10 +13,9 @@ package frc.robot.util; -import static frc.robot.Constants.PowerDistributionConstants.*; - import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants.PowerDistributionConstants; import frc.robot.Ports; import frc.robot.util.Alert.AlertType; import org.littletonrobotics.junction.Logger; @@ -24,7 +23,8 @@ public class PowerMonitoring extends VirtualSubsystem { private PowerDistribution powerDistributionModule = - new PowerDistribution(Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), kPowerModule); + new PowerDistribution( + Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerDistributionConstants.kPowerModule); private int NUM_PDH_CHANNELS = powerDistributionModule.getNumChannels(); private double[] channelCurrents = new double[NUM_PDH_CHANNELS]; @@ -33,12 +33,12 @@ public void periodic() { // Check the total robot current and individual port currents against Constants double totalCurrent = powerDistributionModule.getTotalCurrent(); - if (totalCurrent > kTotalMaxCurrent) { + if (totalCurrent > PowerDistributionConstants.kTotalMaxCurrent) { new Alert("Total current draw exceeds limit!", AlertType.WARNING).set(true); } for (int i = 0; i < NUM_PDH_CHANNELS; i++) { channelCurrents[i] = powerDistributionModule.getCurrent(i); - if (channelCurrents[i] > kMotorPortMaxCurrent) { + if (channelCurrents[i] > PowerDistributionConstants.kMotorPortMaxCurrent) { new Alert("Port " + i + " current draw exceeds limit!", AlertType.WARNING).set(true); } } @@ -46,10 +46,10 @@ public void periodic() { // Compute DRIVE and STEER summed current double driveCurrent = 0.0; double steerCurrent = 0.0; - for (int port : kDrivePowerPorts) { + for (int port : PowerDistributionConstants.kDrivePowerPorts) { driveCurrent += channelCurrents[port]; } - for (int port : kSteerPowerPorts) { + for (int port : PowerDistributionConstants.kSteerPowerPorts) { steerCurrent += channelCurrents[port]; } // Add current monitoring by subsystem here diff --git a/src/test/README b/src/test/README new file mode 100644 index 00000000..9de38271 --- /dev/null +++ b/src/test/README @@ -0,0 +1 @@ +Directory containing unit tests. \ No newline at end of file diff --git a/src/test/RobotContainerTest.java b/src/test/RobotContainerTest.java new file mode 100644 index 00000000..a29ba475 --- /dev/null +++ b/src/test/RobotContainerTest.java @@ -0,0 +1,26 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.littletonrobotics.frc2024; + +import static org.junit.jupiter.api.Assertions.fail; + +import org.junit.jupiter.api.Test; + +public class RobotContainerTest { + + @Test + public void createRobotContainer() { + // Instantiate RobotContainer + try { + new RobotContainer(); + } catch (Exception e) { + e.printStackTrace(); + fail("Failed to instantiate RobotContainer, see stack trace above."); + } + } +} From bf3d564e4cdf0402ea1f889608a9886734ae2bb0 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 10 Oct 2024 17:49:32 -0700 Subject: [PATCH 14/57] Moving stuff around modified: src/main/java/frc/robot/Constants.java deleted: src/main/java/frc/robot/FieldConstants2024.java deleted: src/main/java/frc/robot/Ports.java modified: src/main/java/frc/robot/README modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java modified: src/main/java/frc/robot/subsystems/swervedrive/Vision.java deleted: src/main/java/frc/robot/util/AllianceFlipUtil.java modified: src/main/java/frc/robot/util/PowerMonitoring.java --- src/main/java/frc/robot/Constants.java | 20 +- .../java/frc/robot/FieldConstants2024.java | 201 ------------------ src/main/java/frc/robot/Ports.java | 66 ------ src/main/java/frc/robot/README | 7 - src/main/java/frc/robot/Robot.java | 5 +- src/main/java/frc/robot/RobotContainer.java | 49 +++++ .../swervedrive/SwerveSubsystem.java | 162 +++++++------- .../robot/subsystems/swervedrive/Vision.java | 26 +-- .../java/frc/robot/util/AllianceFlipUtil.java | 105 --------- .../java/frc/robot/util/PowerMonitoring.java | 22 +- 10 files changed, 168 insertions(+), 495 deletions(-) delete mode 100644 src/main/java/frc/robot/FieldConstants2024.java delete mode 100644 src/main/java/frc/robot/Ports.java delete mode 100644 src/main/java/frc/robot/util/AllianceFlipUtil.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9dfbd790..38c4c6c6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -18,6 +18,8 @@ package frc.robot; import com.pathplanner.lib.util.PIDConstants; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; @@ -89,13 +91,17 @@ public static void main(String... args) { /***************************************************************************/ /* The remainder of this file contains physical and/or software constants for the various subsystems of the robot */ - /** General Constants */ + /** General Constants ********************************* */ private static RobotType robotType = getRobot(); public static final double loopPeriodSecs = 0.02; public static final boolean tuningMode = false; - /** Physical Constants for Robot Operation */ + /** AprilTag field layout -- SEASON SPECIFIC! */ + public static final AprilTagFieldLayout aprilTagFieldLayout = + AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + + /** Physical Constants for Robot Operation ************ */ public static final class PhysicalConstants { public static final double kRobotMass = (148 - 20.3) * 0.453592; // 32lbs * kg per pound public static final Matter kChassis = @@ -105,8 +111,8 @@ public static final class PhysicalConstants { // Maximum speed of the robot in meters per second, used to limit acceleration. } - /** Power Distribution Module Constants */ - public static final class PowerDistributionConstants { + /** Power Distribution Module Constants *************** */ + public static final class PowerConstants { // Set this to either kRev or kCTRE for the type of Power Distribution Module public static final ModuleType kPowerModule = ModuleType.kRev; @@ -125,20 +131,20 @@ public static final class PowerDistributionConstants { // public static final int[] kElevatorPowerPorts = {9, 10}; } - /** Autonomous Action Constants */ + /** Autonomous Action Constants *********************** */ public static final class AutonConstants { public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0); public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01); } - /** Drive Base Constants */ + /** Drive Base Constants ****************************** */ public static final class DrivebaseConstants { // Hold time on motor brakes when disabled public static final double WHEEL_LOCK_TIME = 10; // seconds } - /** Operator Constants */ + /** Operator Constants ******************************** */ public static class OperatorConstants { // Joystick Deadband diff --git a/src/main/java/frc/robot/FieldConstants2024.java b/src/main/java/frc/robot/FieldConstants2024.java deleted file mode 100644 index a8fd80ce..00000000 --- a/src/main/java/frc/robot/FieldConstants2024.java +++ /dev/null @@ -1,201 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot; - -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Filesystem; -import java.io.IOException; -import java.nio.file.Path; -import lombok.Getter; - -/** - * Contains various field dimensions and useful reference points. Dimensions are in meters, and sets - * of corners start in the lower left moving clockwise. All units in Meters
- *
- * - *

All translations and poses are stored with the origin at the rightmost point on the BLUE - * ALLIANCE wall.
- *
- * Length refers to the x direction (as described by wpilib)
- * Width refers to the y direction (as described by wpilib) - */ -public class FieldConstants2024 { - public static final double fieldLength = Units.inchesToMeters(651.223); - public static final double fieldWidth = Units.inchesToMeters(323.277); - public static final double wingX = Units.inchesToMeters(229.201); - public static final double podiumX = Units.inchesToMeters(126.75); - public static final double startingLineX = Units.inchesToMeters(74.111); - - public static final Translation2d ampCenter = - new Translation2d(Units.inchesToMeters(72.455), fieldWidth); - - /** Staging locations for each note */ - public static final class StagingLocations { - public static final double centerlineX = fieldLength / 2.0; - - // need to update - public static final double centerlineFirstY = Units.inchesToMeters(29.638); - public static final double centerlineSeparationY = Units.inchesToMeters(66); - public static final double spikeX = Units.inchesToMeters(114); - // need - public static final double spikeFirstY = Units.inchesToMeters(161.638); - public static final double spikeSeparationY = Units.inchesToMeters(57); - - public static final Translation2d[] centerlineTranslations = new Translation2d[5]; - public static final Translation2d[] spikeTranslations = new Translation2d[3]; - - static { - for (int i = 0; i < centerlineTranslations.length; i++) { - centerlineTranslations[i] = - new Translation2d(centerlineX, centerlineFirstY + (i * centerlineSeparationY)); - } - } - - static { - for (int i = 0; i < spikeTranslations.length; i++) { - spikeTranslations[i] = new Translation2d(spikeX, spikeFirstY + (i * spikeSeparationY)); - } - } - } - - /** Each corner of the speaker * */ - public static final class Speaker { - - // corners (blue alliance origin) - public static final Translation3d topRightSpeaker = - new Translation3d( - Units.inchesToMeters(18.055), - Units.inchesToMeters(238.815), - Units.inchesToMeters(83.091)); - - public static final Translation3d topLeftSpeaker = - new Translation3d( - Units.inchesToMeters(18.055), - Units.inchesToMeters(197.765), - Units.inchesToMeters(83.091)); - - public static final Translation3d bottomRightSpeaker = - new Translation3d(0.0, Units.inchesToMeters(238.815), Units.inchesToMeters(78.324)); - public static final Translation3d bottomLeftSpeaker = - new Translation3d(0.0, Units.inchesToMeters(197.765), Units.inchesToMeters(78.324)); - - /** Center of the speaker opening (blue alliance) */ - public static final Translation3d centerSpeakerOpening = - bottomLeftSpeaker.interpolate(topRightSpeaker, 0.5); - } - - public static final class Subwoofer { - public static final Pose2d ampFaceCorner = - new Pose2d( - Units.inchesToMeters(35.775), - Units.inchesToMeters(239.366), - Rotation2d.fromDegrees(-120)); - - public static final Pose2d sourceFaceCorner = - new Pose2d( - Units.inchesToMeters(35.775), - Units.inchesToMeters(197.466), - Rotation2d.fromDegrees(120)); - - public static final Pose2d centerFace = - new Pose2d( - Units.inchesToMeters(35.775), - Units.inchesToMeters(218.416), - Rotation2d.fromDegrees(180)); - } - - public static final class Stage { - public static final Pose2d podiumLeg = - new Pose2d(Units.inchesToMeters(126.75), Units.inchesToMeters(161.638), new Rotation2d()); - public static final Pose2d ampLeg = - new Pose2d( - Units.inchesToMeters(220.873), - Units.inchesToMeters(212.425), - Rotation2d.fromDegrees(-30)); - public static final Pose2d sourceLeg = - new Pose2d( - Units.inchesToMeters(220.873), - Units.inchesToMeters(110.837), - Rotation2d.fromDegrees(30)); - - public static final Pose2d centerPodiumAmpChain = - new Pose2d( - podiumLeg.getTranslation().interpolate(ampLeg.getTranslation(), 0.5), - Rotation2d.fromDegrees(120.0)); - public static final Pose2d centerAmpSourceChain = - new Pose2d( - ampLeg.getTranslation().interpolate(sourceLeg.getTranslation(), 0.5), new Rotation2d()); - public static final Pose2d centerSourcePodiumChain = - new Pose2d( - sourceLeg.getTranslation().interpolate(podiumLeg.getTranslation(), 0.5), - Rotation2d.fromDegrees(240.0)); - public static final Pose2d center = - new Pose2d(Units.inchesToMeters(192.55), Units.inchesToMeters(161.638), new Rotation2d()); - public static final double centerToChainDistance = - center.getTranslation().getDistance(centerPodiumAmpChain.getTranslation()); - } - - public static final class Amp { - public static final Translation2d ampTapeTopCorner = - new Translation2d(Units.inchesToMeters(130.0), Units.inchesToMeters(305.256)); - public static final double ampBottomY = fieldWidth - Units.inchesToMeters(17.75); - } - - public static final double aprilTagWidth = Units.inchesToMeters(6.50); - public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; - - @Getter - public enum AprilTagLayoutType { - OFFICIAL("2024-official"), - SPEAKERS_ONLY("2024-speakers"), - AMPS_ONLY("2024-amps"), - WPI("2024-wpi"); - - private AprilTagLayoutType(String name) { - if (Constants.disableHAL) { - layout = null; - } else { - try { - layout = - new AprilTagFieldLayout( - Path.of(Filesystem.getDeployDirectory().getPath(), "apriltags", name + ".json")); - } catch (IOException e) { - throw new RuntimeException(e); - } - } - if (layout == null) { - layoutString = ""; - } else { - try { - layoutString = new ObjectMapper().writeValueAsString(layout); - } catch (JsonProcessingException e) { - throw new RuntimeException( - "Failed to serialize AprilTag layout JSON " + toString() + "for Northstar"); - } - } - } - - private final AprilTagFieldLayout layout; - private final String layoutString; - } -} diff --git a/src/main/java/frc/robot/Ports.java b/src/main/java/frc/robot/Ports.java deleted file mode 100644 index 0511d5cb..00000000 --- a/src/main/java/frc/robot/Ports.java +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2024 FRC 1678 -// https://github.com/frc1678 -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot; - -import frc.robot.util.CanDeviceId; - -/** List of Channel and CAN IDs */ -public class Ports { - - /* DRIVETRAIN CAN DEVICE IDS */ - // This is the default setup for the Az-RBSI swerve base - // Swerve Modules go: - // FL,FR,BL,BR - // - // 0 1 - // 2 3 - public static final CanDeviceId FL_DRIVE = new CanDeviceId(1, "canivore"); - public static final CanDeviceId FL_ROTATION = new CanDeviceId(2, "canivore"); - public static final CanDeviceId FL_CANCODER = new CanDeviceId(3, "canivore"); - - public static final CanDeviceId FR_DRIVE = new CanDeviceId(4, "canivore"); - public static final CanDeviceId FR_ROTATION = new CanDeviceId(5, "canivore"); - public static final CanDeviceId FR_CANCODER = new CanDeviceId(6, "canivore"); - - public static final CanDeviceId BL_DRIVE = new CanDeviceId(7, "canivore"); - public static final CanDeviceId BL_ROTATION = new CanDeviceId(8, "canivore"); - public static final CanDeviceId BL_CANCODER = new CanDeviceId(9, "canivore"); - - public static final CanDeviceId BR_DRIVE = new CanDeviceId(10, "canivore"); - public static final CanDeviceId BR_ROTATION = new CanDeviceId(11, "canivore"); - public static final CanDeviceId BR_CANCODER = new CanDeviceId(12, "canivore"); - - public static final CanDeviceId PIGEON = new CanDeviceId(13, "canivore"); - - /* POWER DISTRIBUTION CAN ID */ - public static final CanDeviceId POWER_CAN_DEVICE_ID = new CanDeviceId(1, ""); - - /* SUBSYSTEM CAN DEVICE IDS */ - // This is where mechanism subsystem devices are defined - // Example: - // public static final CanDeviceId ELEVATOR_MAIN = new CanDeviceId(3, ""); - - /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */ - // This is where digital I/O feedback devices are defined - // Example: - // public static final int ELEVATOR_BOTTOM_LIMIT = 3; - - /* LINEAR SERVO PWM CHANNELS */ - // This is where PWM-controlled devices (actuators, servos, pneumatics, etc.) - // are defined - // Example: - // public static final int INTAKE_SERVO = 0; -} diff --git a/src/main/java/frc/robot/README b/src/main/java/frc/robot/README index 5abf1169..0553266f 100644 --- a/src/main/java/frc/robot/README +++ b/src/main/java/frc/robot/README @@ -18,13 +18,6 @@ Constants.java importing only the constants needed for a particular software module. This file MUST be modified to include the proper robot constants in the code. -FieldConstants2024.java - Dimensions of the field and 2024 ("Crescendo") specific field elements. - An updated field constants file will be made available each year following - FRC Kickoff. Teams should feel free to add specific field information to - this file they feel is useful for developing commands and/or autonomous - actions. - Main.java This is the file run by the RoboRIO virtual machine. Do NOT alter this file. diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index acec29a9..cdcb893c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,7 +17,8 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.Constants.PowerDistributionConstants; +import frc.robot.Constants.PowerConstants; +import frc.robot.RobotContainer.Ports; import frc.robot.util.VirtualSubsystem; import java.io.File; import java.io.IOException; @@ -76,7 +77,7 @@ public void robotInit() { Logger.addDataReceiver(new WPILOGWriter()); Logger.addDataReceiver(new NT4Publisher()); LoggedPowerDistribution.getInstance( - Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerDistributionConstants.kPowerModule); + Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerConstants.kPowerModule); break; case SIM: diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3cf66ce8..2c1a6c7d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,7 @@ import frc.robot.Constants.OperatorConstants; import frc.robot.commands.swervedrive.drivebase.AbsoluteDriveAdv; import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.util.CanDeviceId; import java.io.File; public class RobotContainer { @@ -127,4 +128,52 @@ public void setDriveMode() { public void setMotorBrake(boolean brake) { m_drivebase.setMotorBrake(brake); } + + /** List of Channel and CAN IDs ****************************************** */ + public static class Ports { + + /* DRIVETRAIN CAN DEVICE IDS */ + // This is the default setup for the Az-RBSI swerve base + // Swerve Modules go: + // FL,FR,BL,BR + // + // 0 1 + // 2 3 + public static final CanDeviceId FL_DRIVE = new CanDeviceId(1, "canivore"); + public static final CanDeviceId FL_ROTATION = new CanDeviceId(2, "canivore"); + public static final CanDeviceId FL_CANCODER = new CanDeviceId(3, "canivore"); + + public static final CanDeviceId FR_DRIVE = new CanDeviceId(4, "canivore"); + public static final CanDeviceId FR_ROTATION = new CanDeviceId(5, "canivore"); + public static final CanDeviceId FR_CANCODER = new CanDeviceId(6, "canivore"); + + public static final CanDeviceId BL_DRIVE = new CanDeviceId(7, "canivore"); + public static final CanDeviceId BL_ROTATION = new CanDeviceId(8, "canivore"); + public static final CanDeviceId BL_CANCODER = new CanDeviceId(9, "canivore"); + + public static final CanDeviceId BR_DRIVE = new CanDeviceId(10, "canivore"); + public static final CanDeviceId BR_ROTATION = new CanDeviceId(11, "canivore"); + public static final CanDeviceId BR_CANCODER = new CanDeviceId(12, "canivore"); + + public static final CanDeviceId PIGEON = new CanDeviceId(13, "canivore"); + + /* POWER DISTRIBUTION CAN ID */ + public static final CanDeviceId POWER_CAN_DEVICE_ID = new CanDeviceId(1, ""); + + /* SUBSYSTEM CAN DEVICE IDS */ + // This is where mechanism subsystem devices are defined + // Example: + // public static final CanDeviceId ELEVATOR_MAIN = new CanDeviceId(3, ""); + + /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */ + // This is where digital I/O feedback devices are defined + // Example: + // public static final int ELEVATOR_BOTTOM_LIMIT = 3; + + /* LINEAR SERVO PWM CHANNELS */ + // This is where PWM-controlled devices (actuators, servos, pneumatics, etc.) + // are defined + // Example: + // public static final int INTAKE_SERVO = 0; + } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 1de53172..4db7df27 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -15,7 +15,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. // -// NOTE: This module from the YAGSL Example Project +// NOTE: This module based on the YAGSL Example Project package frc.robot.subsystems.swervedrive; @@ -25,8 +25,6 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.ReplanningConfig; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -41,6 +39,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +import frc.robot.Constants; import frc.robot.Constants.AutonConstants; import frc.robot.Constants.PhysicalConstants; import java.io.File; @@ -60,16 +59,11 @@ /** SwerveSubsystem module */ public class SwerveSubsystem extends SubsystemBase { - /** PhotonVision class to keep an accurate odometry. */ - private Vision vision; - /** Swerve drive object. */ + /** Swerve drive object */ private final SwerveDrive swerveDrive; - - /** AprilTag field layout. */ - private final AprilTagFieldLayout aprilTagFieldLayout = - AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - - /** Enable vision odometry updates while driving. */ + /** PhotonVision class to keep an accurate odometry */ + private Vision vision; + /** Enable vision odometry updates while driving */ private final boolean visionDriveTest = false; /** @@ -180,77 +174,6 @@ public void setupPathPlanner() { ); } - /** - * Get the distance to the speaker. - * - * @return Distance to speaker in meters. - */ - public double getDistanceToSpeaker() { - int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; - // Taken from PhotonUtils.getDistanceToPose - Pose3d speakerAprilTagPose = aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); - return getPose().getTranslation().getDistance(speakerAprilTagPose.toPose2d().getTranslation()); - } - - /** - * Get the yaw to aim at the speaker. - * - * @return {@link Rotation2d} of which you need to achieve. - */ - public Rotation2d getSpeakerYaw() { - int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; - // Taken from PhotonUtils.getYawToPose() - Pose3d speakerAprilTagPose = aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); - Translation2d relativeTrl = - speakerAprilTagPose.toPose2d().relativeTo(getPose()).getTranslation(); - return new Rotation2d(relativeTrl.getX(), relativeTrl.getY()) - .plus(swerveDrive.getOdometryHeading()); - } - - /** - * Aim the robot at the speaker. - * - * @param tolerance Tolerance in degrees. - * @return Command to turn the robot to the speaker. - */ - public Command aimAtSpeaker(double tolerance) { - SwerveController controller = swerveDrive.getSwerveController(); - return run(() -> { - drive( - ChassisSpeeds.fromFieldRelativeSpeeds( - 0, - 0, - controller.headingCalculate( - getHeading().getRadians(), getSpeakerYaw().getRadians()), - getHeading())); - }) - .until(() -> getSpeakerYaw().minus(getHeading()).getDegrees() < tolerance); - } - - /** - * Aim the robot at the target returned by PhotonVision. - * - * @param camera {@link PhotonCamera} to communicate with. - * @return A {@link Command} which will run the alignment. - */ - public Command aimAtTarget(PhotonCamera camera) { - - return run( - () -> { - PhotonPipelineResult result = camera.getLatestResult(); - if (result.hasTargets()) { - drive( - getTargetSpeeds( - 0, - 0, - Rotation2d.fromDegrees( - result - .getBestTarget() - .getYaw()))); // Not sure if this will work, more math may be required. - } - }); - } - /** * Get the path follower with events. * @@ -628,4 +551,77 @@ public void addFakeVisionReading() { swerveDrive.addVisionMeasurement( new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp()); } + + /*****************************************************************/ + /** 2024 SEASON-SPECIFIC FUNCTIONS, INCLUDED AS EXAMPLES */ + /** + * Get the distance to the speaker. + * + * @return Distance to speaker in meters. + */ + public double getDistanceToSpeaker() { + int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; + // Taken from PhotonUtils.getDistanceToPose + Pose3d speakerAprilTagPose = Constants.aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); + return getPose().getTranslation().getDistance(speakerAprilTagPose.toPose2d().getTranslation()); + } + + /** + * Get the yaw to aim at the speaker. + * + * @return {@link Rotation2d} of which you need to achieve. + */ + public Rotation2d getSpeakerYaw() { + int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; + // Taken from PhotonUtils.getYawToPose() + Pose3d speakerAprilTagPose = Constants.aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); + Translation2d relativeTrl = + speakerAprilTagPose.toPose2d().relativeTo(getPose()).getTranslation(); + return new Rotation2d(relativeTrl.getX(), relativeTrl.getY()) + .plus(swerveDrive.getOdometryHeading()); + } + + /** + * Aim the robot at the speaker. + * + * @param tolerance Tolerance in degrees. + * @return Command to turn the robot to the speaker. + */ + public Command aimAtSpeaker(double tolerance) { + SwerveController controller = swerveDrive.getSwerveController(); + return run(() -> { + drive( + ChassisSpeeds.fromFieldRelativeSpeeds( + 0, + 0, + controller.headingCalculate( + getHeading().getRadians(), getSpeakerYaw().getRadians()), + getHeading())); + }) + .until(() -> getSpeakerYaw().minus(getHeading()).getDegrees() < tolerance); + } + + /** + * Aim the robot at the target returned by PhotonVision. + * + * @param camera {@link PhotonCamera} to communicate with. + * @return A {@link Command} which will run the alignment. + */ + public Command aimAtTarget(PhotonCamera camera) { + + return run( + () -> { + PhotonPipelineResult result = camera.getLatestResult(); + if (result.hasTargets()) { + drive( + getTargetSpeeds( + 0, + 0, + Rotation2d.fromDegrees( + result + .getBestTarget() + .getYaw()))); // Not sure if this will work, more math may be required. + } + }); + } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index 41a881f5..05f7a6f5 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -15,12 +15,10 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. // -// NOTE: This module from the YAGSL Example Project +// NOTE: This module based on the YAGSL Example Project package frc.robot.subsystems.swervedrive; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; @@ -34,6 +32,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import frc.robot.Constants; import frc.robot.Robot; import java.awt.Desktop; import java.util.ArrayList; @@ -61,9 +60,6 @@ */ public class Vision { - /** April Tag Field Layout of the year. */ - public static final AprilTagFieldLayout fieldLayout = - AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo); /** Photon Vision Simulation */ public VisionSystemSim visionSim; /** Count of times that the odom thinks we're more than 10meters away from the april tag. */ @@ -87,7 +83,7 @@ public Vision(Supplier currentPose, Field2d field) { if (Robot.isSimulation()) { visionSim = new VisionSystemSim("Vision"); - visionSim.addAprilTags(fieldLayout); + visionSim.addAprilTags(Constants.aprilTagFieldLayout); for (Cameras c : Cameras.values()) { c.addToVisionSim(visionSim); @@ -106,12 +102,15 @@ public Vision(Supplier currentPose, Field2d field) { * @return The target pose of the AprilTag. */ public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) { - Optional aprilTagPose3d = fieldLayout.getTagPose(aprilTag); + Optional aprilTagPose3d = Constants.aprilTagFieldLayout.getTagPose(aprilTag); if (aprilTagPose3d.isPresent()) { return aprilTagPose3d.get().toPose2d().transformBy(robotOffset); } else { throw new RuntimeException( - "Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString()); + "Cannot get AprilTag " + + aprilTag + + " from field " + + Constants.aprilTagFieldLayout.toString()); } } @@ -258,7 +257,7 @@ public PhotonPipelineResult getLatestResult(Cameras camera) { * @return Distance */ public double getDistanceFromAprilTag(int id) { - Optional tag = fieldLayout.getTagPose(id); + Optional tag = Constants.aprilTagFieldLayout.getTagPose(id); return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())) .orElse(-1.0); } @@ -322,8 +321,9 @@ public void updateVisionField() { List poses = new ArrayList<>(); for (PhotonTrackedTarget target : targets) { - if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) { - Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d(); + if (Constants.aprilTagFieldLayout.getTagPose(target.getFiducialId()).isPresent()) { + Pose2d targetPose = + Constants.aprilTagFieldLayout.getTagPose(target.getFiducialId()).get().toPose2d(); poses.add(targetPose); } } @@ -404,7 +404,7 @@ enum Cameras { poseEstimator = new PhotonPoseEstimator( - Vision.fieldLayout, + Constants.aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera, robotToCamTransform); diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java deleted file mode 100644 index 550d7e9c..00000000 --- a/src/main/java/frc/robot/util/AllianceFlipUtil.java +++ /dev/null @@ -1,105 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.util; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import frc.robot.FieldConstants2024; - -/** Utility functions for flipping from the blue to red alliance. */ -public class AllianceFlipUtil { - /** Flips an x coordinate to the correct side of the field based on the current alliance color. */ - public static double apply(double xCoordinate) { - if (shouldFlip()) { - return FieldConstants2024.fieldLength - xCoordinate; - } else { - return xCoordinate; - } - } - - /** Flips a translation to the correct side of the field based on the current alliance color. */ - public static Translation2d apply(Translation2d translation) { - if (shouldFlip()) { - return new Translation2d(apply(translation.getX()), translation.getY()); - } else { - return translation; - } - } - - /** Flips a rotation based on the current alliance color. */ - public static Rotation2d apply(Rotation2d rotation) { - if (shouldFlip()) { - return new Rotation2d(-rotation.getCos(), rotation.getSin()); - } else { - return rotation; - } - } - - /** Flips a pose to the correct side of the field based on the current alliance color. */ - public static Pose2d apply(Pose2d pose) { - if (shouldFlip()) { - return new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())); - } else { - return pose; - } - } - - public static Translation3d apply(Translation3d translation3d) { - if (shouldFlip()) { - return new Translation3d( - apply(translation3d.getX()), translation3d.getY(), translation3d.getZ()); - } else { - return translation3d; - } - } - - // /** - // * Flips a trajectory state to the correct side of the field based on the current alliance - // color. - // */ - // public static VehicleState apply(VehicleState state) { - // if (shouldFlip()) { - // return VehicleState.newBuilder() - // .setX(apply(state.getX())) - // .setY(state.getY()) - // .setTheta(apply(new Rotation2d(state.getTheta())).getRadians()) - // .setVx(-state.getVx()) - // .setVy(state.getVy()) - // .setOmega(-state.getOmega()) - // .addAllModuleForces( - // state.getModuleForcesList().stream() - // .map( - // forces -> - // VehicleTrajectoryServiceOuterClass.ModuleForce.newBuilder() - // .setFx(-forces.getFx()) - // .setFy(forces.getFy()) - // .build()) - // .toList()) - // .build(); - // } else { - // return state; - // } - // } - - public static boolean shouldFlip() { - return DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; - } -} diff --git a/src/main/java/frc/robot/util/PowerMonitoring.java b/src/main/java/frc/robot/util/PowerMonitoring.java index 39247764..0cbf48eb 100644 --- a/src/main/java/frc/robot/util/PowerMonitoring.java +++ b/src/main/java/frc/robot/util/PowerMonitoring.java @@ -15,30 +15,30 @@ import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants.PowerDistributionConstants; -import frc.robot.Ports; +import frc.robot.Constants.PowerConstants; +import frc.robot.RobotContainer.Ports; import frc.robot.util.Alert.AlertType; import org.littletonrobotics.junction.Logger; public class PowerMonitoring extends VirtualSubsystem { - private PowerDistribution powerDistributionModule = + private PowerDistribution m_powerModule = new PowerDistribution( - Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerDistributionConstants.kPowerModule); - private int NUM_PDH_CHANNELS = powerDistributionModule.getNumChannels(); + Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerConstants.kPowerModule); + private int NUM_PDH_CHANNELS = m_powerModule.getNumChannels(); private double[] channelCurrents = new double[NUM_PDH_CHANNELS]; /** Periodic Method */ public void periodic() { // Check the total robot current and individual port currents against Constants - double totalCurrent = powerDistributionModule.getTotalCurrent(); - if (totalCurrent > PowerDistributionConstants.kTotalMaxCurrent) { + double totalCurrent = m_powerModule.getTotalCurrent(); + if (totalCurrent > PowerConstants.kTotalMaxCurrent) { new Alert("Total current draw exceeds limit!", AlertType.WARNING).set(true); } for (int i = 0; i < NUM_PDH_CHANNELS; i++) { - channelCurrents[i] = powerDistributionModule.getCurrent(i); - if (channelCurrents[i] > PowerDistributionConstants.kMotorPortMaxCurrent) { + channelCurrents[i] = m_powerModule.getCurrent(i); + if (channelCurrents[i] > PowerConstants.kMotorPortMaxCurrent) { new Alert("Port " + i + " current draw exceeds limit!", AlertType.WARNING).set(true); } } @@ -46,10 +46,10 @@ public void periodic() { // Compute DRIVE and STEER summed current double driveCurrent = 0.0; double steerCurrent = 0.0; - for (int port : PowerDistributionConstants.kDrivePowerPorts) { + for (int port : PowerConstants.kDrivePowerPorts) { driveCurrent += channelCurrents[port]; } - for (int port : PowerDistributionConstants.kSteerPowerPorts) { + for (int port : PowerConstants.kSteerPowerPorts) { steerCurrent += channelCurrents[port]; } // Add current monitoring by subsystem here From a7efee69774d82aeaf7feab06e3592ca575df55a Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 10 Oct 2024 20:55:34 -0700 Subject: [PATCH 15/57] Parsing out the vision into a separate subsystem modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java renamed: src/main/java/frc/robot/subsystems/swervedrive/Vision.java -> src/main/java/frc/robot/subsystems/vision/Vision.java new file: src/main/java/frc/robot/subsystems/vision/VisionIO.java new file: src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java --- src/main/java/frc/robot/Constants.java | 41 ++++++++++ src/main/java/frc/robot/RobotContainer.java | 26 +++++++ .../swervedrive/SwerveSubsystem.java | 1 + .../{swervedrive => vision}/Vision.java | 2 +- .../frc/robot/subsystems/vision/VisionIO.java | 54 +++++++++++++ .../subsystems/vision/VisionIOPhoton.java | 77 +++++++++++++++++++ 6 files changed, 200 insertions(+), 1 deletion(-) rename src/main/java/frc/robot/subsystems/{swervedrive => vision}/Vision.java (99%) create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIO.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 38c4c6c6..1754312e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -17,15 +17,21 @@ package frc.robot; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.ObjectMapper; import com.pathplanner.lib.util.PIDConstants; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.RobotBase; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; +import java.io.IOException; +import java.nio.file.Path; +import lombok.Getter; import swervelib.math.Matter; /** @@ -153,4 +159,39 @@ public static class OperatorConstants { public static final double RIGHT_X_DEADBAND = 0.1; public static final double TURN_CONSTANT = 6; } + + @Getter + public enum AprilTagLayoutType { + OFFICIAL("2024-official"), + SPEAKERS_ONLY("2024-speakers"), + AMPS_ONLY("2024-amps"), + WPI("2024-wpi"); + + private AprilTagLayoutType(String name) { + if (Constants.disableHAL) { + layout = null; + } else { + try { + layout = + new AprilTagFieldLayout( + Path.of(Filesystem.getDeployDirectory().getPath(), "apriltags", name + ".json")); + } catch (IOException e) { + throw new RuntimeException(e); + } + } + if (layout == null) { + layoutString = ""; + } else { + try { + layoutString = new ObjectMapper().writeValueAsString(layout); + } catch (JsonProcessingException e) { + throw new RuntimeException( + "Failed to serialize AprilTag layout JSON " + toString() + "for Northstar"); + } + } + } + + private final AprilTagFieldLayout layout; + private final String layoutString; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2c1a6c7d..dd2e93fa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,9 @@ package frc.robot; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; @@ -176,4 +179,27 @@ public static class Ports { // Example: // public static final int INTAKE_SERVO = 0; } + + /* Vision Constants and Camera Posses */ + public static class VisionConstants { + public static final double ambiguityThreshold = 0.4; + public static final double targetLogTimeSecs = 0.1; + public static final double fieldBorderMargin = 0.5; + public static final double zMargin = 0.75; + public static final double xyStdDevCoefficient = 0.005; + public static final double thetaStdDevCoefficient = 0.01; + + public static final Pose3d[] cameraPoses = + switch (Constants.getRobot()) { + case COMPBOT -> new Pose3d[] { + new Pose3d( + Units.inchesToMeters(-1.0), + Units.inchesToMeters(0), + Units.inchesToMeters(23.5), + new Rotation3d(0.0, Units.degreesToRadians(-20), 0.0)), + }; + case DEVBOT -> new Pose3d[] {}; + default -> new Pose3d[] {}; + }; + } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 4db7df27..5ec62641 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -42,6 +42,7 @@ import frc.robot.Constants; import frc.robot.Constants.AutonConstants; import frc.robot.Constants.PhysicalConstants; +import frc.robot.subsystems.vision.Vision; import java.io.File; import java.util.function.DoubleSupplier; import org.photonvision.PhotonCamera; diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java similarity index 99% rename from src/main/java/frc/robot/subsystems/swervedrive/Vision.java rename to src/main/java/frc/robot/subsystems/vision/Vision.java index 05f7a6f5..8e965988 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -17,7 +17,7 @@ // // NOTE: This module based on the YAGSL Example Project -package frc.robot.subsystems.swervedrive; +package frc.robot.subsystems.vision; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java new file mode 100644 index 00000000..5fd611f5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -0,0 +1,54 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.vision; + +import java.util.ArrayList; +import java.util.List; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; +import org.photonvision.targeting.PhotonTrackedTarget; + +public interface VisionIO { + + class AprilTagVisionIOInputs implements LoggableInputs { + public String camname = ""; + public double latency = 0.0; + public double timestamp = 0.0; + public List targets = new ArrayList() {}; + + public long fps = 0; + + @Override + public void toLog(LogTable table) { + table.put("Latency", latency); + table.put("Timestamp", timestamp); + table.put("TargetCount", targets.size()); + for (int i = 0; i < targets.size(); i++) { + table.put("Target/" + i, targets.get(i)); + } + table.put("Fps", fps); + } + + @Override + public void fromLog(LogTable table) { + latency = table.get("Latency", 0.0); + timestamp = table.get("Timestamp", 0.0); + int targetCount = table.get("TargetCount", 0); + targets = new ArrayList(targetCount); + fps = table.get("Fps", 0); + } + } + + default void updateInputs(AprilTagVisionIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java new file mode 100644 index 00000000..d43b98f0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java @@ -0,0 +1,77 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants.AprilTagLayoutType; +import frc.robot.util.Alert; +import java.util.List; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonTrackedTarget; + +public class VisionIOPhoton implements VisionIO { + + private final Supplier aprilTagTypeSupplier; + private AprilTagLayoutType lastAprilTagType = null; + + private static final double disconnectedTimeout = 0.5; + private final Alert disconnectedAlert; + private final Timer disconnectedTimer = new Timer(); + public final PhotonCamera camera; + public final String camname; + + public VisionIOPhoton(Supplier aprilTagTypeSupplier, String camname) { + this.aprilTagTypeSupplier = aprilTagTypeSupplier; + this.camname = camname; + this.camera = new PhotonCamera(camname); + + disconnectedAlert = new Alert("No data from \"" + camname + "\"", Alert.AlertType.ERROR); + disconnectedTimer.start(); + } + + public void updateInputs(AprilTagVisionIOInputs inputs) { + // Get observations + var result = camera.getLatestResult(); + + // Log the entire result for each camera to AdvantageKit + Logger.recordOutput("PhotonVision/" + camname, result); // result.getLatencyMillis()); + + // Put the relevant information into the `inputs` + inputs.camname = camname; + inputs.latency = result.getLatencyMillis(); + inputs.timestamp = result.getTimestampSeconds(); + + if (result.hasTargets()) { + List targets = result.getTargets(); + inputs.targets = targets; + for (PhotonTrackedTarget target : targets) { + // Get information from target. + int targetID = target.getFiducialId(); + double poseAmbiguity = target.getPoseAmbiguity(); + Transform3d bestCameraToTarget = target.getBestCameraToTarget(); + Transform3d alternateCameraToTarget = target.getAlternateCameraToTarget(); + + String logTag = "PhotonVision/Tag" + Integer.toString(targetID); + Logger.recordOutput(logTag + "/PoseAmbiguity", poseAmbiguity); + Logger.recordOutput(logTag + "/BestCameraToTarget", bestCameraToTarget); + Logger.recordOutput(logTag + "/AlternateCameraToTarget", alternateCameraToTarget); + } + } else { + inputs.targets = null; + } + } +} From 0ba542cbfc982499d100d2b71fdc3bb1bc6454c9 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 10 Oct 2024 21:38:13 -0700 Subject: [PATCH 16/57] Explicitly make the flywheel an example subsystem renamed: src/main/java/frc/robot/subsystems/flywheel/Flywheel.java -> src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java renamed: src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java -> src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java renamed: src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java -> src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java renamed: src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java -> src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java renamed: src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java -> src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java new file: src/main/java/frc/robot/subsystems/flywheel_example/README --- .../Flywheel.java | 2 +- .../FlywheelIO.java | 2 +- .../FlywheelIOSim.java | 2 +- .../FlywheelIOSparkMax.java | 2 +- .../FlywheelIOTalonFX.java | 2 +- .../robot/subsystems/flywheel_example/README | 33 +++++++++++++++++++ 6 files changed, 38 insertions(+), 5 deletions(-) rename src/main/java/frc/robot/subsystems/{flywheel => flywheel_example}/Flywheel.java (98%) rename src/main/java/frc/robot/subsystems/{flywheel => flywheel_example}/FlywheelIO.java (96%) rename src/main/java/frc/robot/subsystems/{flywheel => flywheel_example}/FlywheelIOSim.java (97%) rename src/main/java/frc/robot/subsystems/{flywheel => flywheel_example}/FlywheelIOSparkMax.java (98%) rename src/main/java/frc/robot/subsystems/{flywheel => flywheel_example}/FlywheelIOTalonFX.java (98%) create mode 100644 src/main/java/frc/robot/subsystems/flywheel_example/README diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java similarity index 98% rename from src/main/java/frc/robot/subsystems/flywheel/Flywheel.java rename to src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java index 342df9df..325bc9e3 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java @@ -11,7 +11,7 @@ // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. -package frc.robot.subsystems.flywheel; +package frc.robot.subsystems.flywheel_example; import static edu.wpi.first.units.Units.*; diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java similarity index 96% rename from src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java rename to src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java index 98f7624c..4830fc30 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java @@ -11,7 +11,7 @@ // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. -package frc.robot.subsystems.flywheel; +package frc.robot.subsystems.flywheel_example; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java similarity index 97% rename from src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java rename to src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java index 32ffa6f3..3a19b6b3 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java @@ -11,7 +11,7 @@ // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. -package frc.robot.subsystems.flywheel; +package frc.robot.subsystems.flywheel_example; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java similarity index 98% rename from src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java rename to src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java index 787f3693..f9990aae 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java @@ -11,7 +11,7 @@ // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. -package frc.robot.subsystems.flywheel; +package frc.robot.subsystems.flywheel_example; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkLowLevel.MotorType; diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java similarity index 98% rename from src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java rename to src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java index 3f535129..0cd34f41 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java @@ -11,7 +11,7 @@ // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. -package frc.robot.subsystems.flywheel; +package frc.robot.subsystems.flywheel_example; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/README b/src/main/java/frc/robot/subsystems/flywheel_example/README new file mode 100644 index 00000000..7cad088f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel_example/README @@ -0,0 +1,33 @@ +This directory contains an example generic flywheel subsystem, as distributed +with AdvantageKit. The structure of the subsystem directory is: + + * Flywheel.java + The base subsystem definitions and outward-facing (public) hardware- + agnostic functions for interacting with the subsystem. The idea is + that any of the hardware-specific (i.e. motor controller API calls) + functionality is insulated from the rest of the robot code within + the library-specific modules in this directory. + + * FlywheelIO.java + The base subsystem I/O (input/output) interface that contains the + structure and class varibales needed by the library-specific modules. + + * FlywheelIOSim.java + Simulated flywheel module, for use with robot simulations, does not + control actual hardware. + + * FlywheelIOSparkMax.java + An example implementation of a flywheel using Rev SparkMax motor + controllers (and their associated library APIs). + + * FlywheelIOTalonFX.java + An example implementation of a flywheel using CTRE TalonFX motor + controllers (and their associated library APIs). + +-------------------- +When creating a new mechanism subsystem, consider using this generic framework +for public vs. private functions and API calls to make your code more modular +and reusable from year to year. Additionally, the I/O framework given here +interfaces with AdvantageKit's logging to allow for replay of inputs and +testing of code changes in simulation. + \ No newline at end of file From 80109bb5484b2e82f09addc2e38159292df042d4 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 10 Oct 2024 21:55:10 -0700 Subject: [PATCH 17/57] Cleaning up some loose ends modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/README modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/vision/VisionIO.java modified: src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java deleted: src/main/java/frc/robot/util/swerve/ModuleLimits.java --- src/main/java/frc/robot/Constants.java | 19 ++++++++++--------- src/main/java/frc/robot/README | 15 ++++++--------- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/vision/VisionIO.java | 4 ++-- .../subsystems/vision/VisionIOPhoton.java | 2 +- .../frc/robot/util/swerve/ModuleLimits.java | 19 ------------------- 6 files changed, 20 insertions(+), 41 deletions(-) delete mode 100644 src/main/java/frc/robot/util/swerve/ModuleLimits.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1754312e..0371513a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -97,16 +97,12 @@ public static void main(String... args) { /***************************************************************************/ /* The remainder of this file contains physical and/or software constants for the various subsystems of the robot */ - /** General Constants ********************************* */ + /** General Constants **************************************************** */ private static RobotType robotType = getRobot(); public static final double loopPeriodSecs = 0.02; public static final boolean tuningMode = false; - /** AprilTag field layout -- SEASON SPECIFIC! */ - public static final AprilTagFieldLayout aprilTagFieldLayout = - AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - /** Physical Constants for Robot Operation ************ */ public static final class PhysicalConstants { public static final double kRobotMass = (148 - 20.3) * 0.453592; // 32lbs * kg per pound @@ -117,7 +113,7 @@ public static final class PhysicalConstants { // Maximum speed of the robot in meters per second, used to limit acceleration. } - /** Power Distribution Module Constants *************** */ + /** Power Distribution Module Constants ********************************** */ public static final class PowerConstants { // Set this to either kRev or kCTRE for the type of Power Distribution Module @@ -137,20 +133,20 @@ public static final class PowerConstants { // public static final int[] kElevatorPowerPorts = {9, 10}; } - /** Autonomous Action Constants *********************** */ + /** Autonomous Action Constants ****************************************** */ public static final class AutonConstants { public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0); public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01); } - /** Drive Base Constants ****************************** */ + /** Drive Base Constants ************************************************* */ public static final class DrivebaseConstants { // Hold time on motor brakes when disabled public static final double WHEEL_LOCK_TIME = 10; // seconds } - /** Operator Constants ******************************** */ + /** Operator Constants *************************************************** */ public static class OperatorConstants { // Joystick Deadband @@ -160,6 +156,11 @@ public static class OperatorConstants { public static final double TURN_CONSTANT = 6; } + /** AprilTag Field Layout ************************************************ */ + /* SEASON SPECIFIC! */ + public static final AprilTagFieldLayout aprilTagFieldLayout = + AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + @Getter public enum AprilTagLayoutType { OFFICIAL("2024-official"), diff --git a/src/main/java/frc/robot/README b/src/main/java/frc/robot/README index 0553266f..36ba2f23 100644 --- a/src/main/java/frc/robot/README +++ b/src/main/java/frc/robot/README @@ -22,23 +22,20 @@ Main.java This is the file run by the RoboRIO virtual machine. Do NOT alter this file. -Ports.java - A file for enumerating the CANbus and DIO ports utilized by the various - devices on the robot. This file MUST be modified so that the subsystems - can find their proper devices. - Robot.java This file is called by Main.java and directs the operation of the robot according to the currently commanded mode (disabled, autonomous, teleop, - test, and simulation). Care should be taken when modifying this module - to ensure proper operation of the robot. One section that would be useful - to modify is the test mode, where teams can implement pre-match systems + test, and simulation). Care must be taken when modifying this module to + ensure proper operation of the robot. One section that would be useful to + modify is the test mode, where teams can implement pre-match systems testing in a known sequence. RobotContainer.java A file for holding robot subsystems and commands. This file is called from Robot.java and is where all subsystems and commands should be gathered - and organized, including button bindings for operator controls. + and organized including button bindings for operator controls, CANbus and + DIO port enumeration for robot devices, and camera pose information (with + respect to robot center). -------------------- diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dd2e93fa..77054c4a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -180,7 +180,7 @@ public static class Ports { // public static final int INTAKE_SERVO = 0; } - /* Vision Constants and Camera Posses */ + /** Vision Constants and Camera Posses *********************************** */ public static class VisionConstants { public static final double ambiguityThreshold = 0.4; public static final double targetLogTimeSecs = 0.1; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 5fd611f5..4c0246c8 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -21,7 +21,7 @@ public interface VisionIO { - class AprilTagVisionIOInputs implements LoggableInputs { + class VisionIOInputs implements LoggableInputs { public String camname = ""; public double latency = 0.0; public double timestamp = 0.0; @@ -50,5 +50,5 @@ public void fromLog(LogTable table) { } } - default void updateInputs(AprilTagVisionIOInputs inputs) {} + default void updateInputs(VisionIOInputs inputs) {} } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java index d43b98f0..f51e9273 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java @@ -43,7 +43,7 @@ public VisionIOPhoton(Supplier aprilTagTypeSupplier, String disconnectedTimer.start(); } - public void updateInputs(AprilTagVisionIOInputs inputs) { + public void updateInputs(VisionIOInputs inputs) { // Get observations var result = camera.getLatestResult(); diff --git a/src/main/java/frc/robot/util/swerve/ModuleLimits.java b/src/main/java/frc/robot/util/swerve/ModuleLimits.java deleted file mode 100644 index 63282e2e..00000000 --- a/src/main/java/frc/robot/util/swerve/ModuleLimits.java +++ /dev/null @@ -1,19 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.util.swerve; - -public record ModuleLimits( - double maxDriveVelocity, double maxDriveAcceleration, double maxSteeringVelocity) {} From 36fb1681f809716ed969728538558d8d888cfd7a Mon Sep 17 00:00:00 2001 From: Timothy Ellsworth Bowers Date: Tue, 15 Oct 2024 09:39:46 -0700 Subject: [PATCH 18/57] Create dependabot.yml Utilize some of GitHub's features. * Bumps com.peterabeles.gversion from 1.10 to 1.10.3. * Bumps com.diffplug.spotless from 6.12.0 to 6.25.0. * Bumps io.freefair.lombok from 8.4 to 8.10.2. Cleaning after upgrade of spotless --- updated-dependencies: - dependency-name: com.peterabeles.gversion dependency-type: direct:production update-type: version-update:semver-patch - dependency-name: com.diffplug.spotless dependency-type: direct:production update-type: version-update:semver-minor - dependency-name: io.freefair.lombok dependency-type: direct:production update-type: version-update:semver-minor ... Signed-off-by: dependabot[bot] new file: .github/dependabot.yml modified: build.gradle modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java modified: src/main/java/frc/robot/subsystems/vision/Vision.java --- .github/dependabot.yml | 23 +++++++++++++++++++ build.gradle | 12 ++++++---- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/RobotContainer.java | 15 ++++++------ .../swervedrive/SwerveSubsystem.java | 2 ++ .../frc/robot/subsystems/vision/Vision.java | 8 +++++++ 6 files changed, 51 insertions(+), 11 deletions(-) create mode 100644 .github/dependabot.yml diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 00000000..ffd8ba16 --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,23 @@ +# To get started with Dependabot version updates, you'll need to specify which +# package ecosystems to update and where the package manifests are located. +# Please see the documentation for all configuration options: +# https://docs.github.com/code-security/dependabot/dependabot-version-updates/configuration-options-for-the-dependabot.yml-file + +version: 2 +updates: + + # Maintain dependencies for GitHub Actions + - package-ecosystem: "github-actions" + # Workflow files stored in the default location of `.github/workflows`. (You don't need to specify `/.github/workflows` for `directory`. You can use `directory: "/"`.) + directory: "/" + schedule: + interval: "weekly" + + # Maintain dependencies for gradle + - package-ecosystem: "gradle" + directory: "/" # Location of package manifests + registries: "*" + labels: + - "gradle dependencies" + schedule: + interval: "monthly" diff --git a/build.gradle b/build.gradle index 4c01a983..8b0da398 100644 --- a/build.gradle +++ b/build.gradle @@ -1,9 +1,9 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2024.3.2" - id "com.peterabeles.gversion" version "1.10" - id "com.diffplug.spotless" version "6.12.0" - id "io.freefair.lombok" version "8.4" + id "com.peterabeles.gversion" version "1.10.3" + id "com.diffplug.spotless" version "6.25.0" + id "io.freefair.lombok" version "8.10.2" id "com.google.protobuf" version "0.9.4" } @@ -118,7 +118,11 @@ wpi.sim.addDriverstation() // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. jar { - from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from { + configurations.runtimeClasspath.collect { + it.isDirectory() ? it : zipTree(it) + } + } from sourceSets.main.allSource manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0371513a..4b517aa8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -50,12 +50,14 @@ public final class Constants { * etc.) and the operating modes of the code (REAL, SIM, or REPLAY) */ public static boolean disableHAL = false; + /** Enumerate the robot types (add additional bots here) */ public static enum RobotType { DEVBOT, // Development / Alpha / Practice Bot COMPBOT, // Competition robot SIMBOT // Simulated robot } + /** Enumerate the robot operation modes */ public static enum Mode { REAL, // REAL == Running on a real robot diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 77054c4a..2f89da45 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -191,13 +191,14 @@ public static class VisionConstants { public static final Pose3d[] cameraPoses = switch (Constants.getRobot()) { - case COMPBOT -> new Pose3d[] { - new Pose3d( - Units.inchesToMeters(-1.0), - Units.inchesToMeters(0), - Units.inchesToMeters(23.5), - new Rotation3d(0.0, Units.degreesToRadians(-20), 0.0)), - }; + case COMPBOT -> + new Pose3d[] { + new Pose3d( + Units.inchesToMeters(-1.0), + Units.inchesToMeters(0), + Units.inchesToMeters(23.5), + new Rotation3d(0.0, Units.degreesToRadians(-20), 0.0)), + }; case DEVBOT -> new Pose3d[] {}; default -> new Pose3d[] {}; }; diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 5ec62641..65960442 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -62,8 +62,10 @@ public class SwerveSubsystem extends SubsystemBase { /** Swerve drive object */ private final SwerveDrive swerveDrive; + /** PhotonVision class to keep an accurate odometry */ private Vision vision; + /** Enable vision odometry updates while driving */ private final boolean visionDriveTest = false; diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 8e965988..7aa450e9 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -62,12 +62,16 @@ public class Vision { /** Photon Vision Simulation */ public VisionSystemSim visionSim; + /** Count of times that the odom thinks we're more than 10meters away from the april tag. */ private double longDistangePoseEstimationCount = 0; + /** Current pose from the pose estimator using wheel odometry. */ private Supplier currentPose; + /** Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}. */ private final double maximumAmbiguity = 0.25; + /** Field from {@link swervelib.SwerveDrive#field} */ private Field2d field2d; @@ -364,15 +368,19 @@ enum Cameras { /** Latency alert to use when high latency is detected. */ public final Alert latencyAlert; + /** Camera instance for comms. */ public final PhotonCamera camera; + /** Pose estimator for camera. */ public final PhotonPoseEstimator poseEstimator; public final Matrix singleTagStdDevs; public final Matrix multiTagStdDevs; + /** Transform of the camera rotation and translation relative to the center of the robot */ private final Transform3d robotToCamTransform; + /** Simulated camera instance which only exists during simulations. */ public PhotonCameraSim cameraSim; From ccbddafab05feeb0094f542419c6b87082258e47 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Tue, 15 Oct 2024 08:39:08 -0700 Subject: [PATCH 19/57] Integrating YAGSL / PathPlanner deploy files renamed: src/main/deploy/pathplanner2024/autos/Example Auto.auto -> src/main/deploy/pathplanner/autos/Example Auto.auto new file: src/main/deploy/pathplanner/autos/New Auto.auto renamed: src/main/deploy/pathplanner2024/navgrid.json -> src/main/deploy/pathplanner/navgrid.json renamed: src/main/deploy/pathplanner2024/paths/Example Path.path -> src/main/deploy/pathplanner/paths/Example Path.path new file: src/main/deploy/pathplanner/paths/New Path.path renamed: src/main/deploy/swerve/falcon/controllerproperties.json -> src/main/deploy/swerve/controllerproperties.json deleted: src/main/deploy/swerve/falcon/modules/backleft.json deleted: src/main/deploy/swerve/falcon/modules/backright.json deleted: src/main/deploy/swerve/falcon/modules/frontleft.json deleted: src/main/deploy/swerve/falcon/modules/frontright.json deleted: src/main/deploy/swerve/falcon/modules/physicalproperties.json new file: src/main/deploy/swerve/modules/backleft.json new file: src/main/deploy/swerve/modules/backright.json new file: src/main/deploy/swerve/modules/frontleft.json new file: src/main/deploy/swerve/modules/frontright.json new file: src/main/deploy/swerve/modules/physicalproperties.json renamed: src/main/deploy/swerve/falcon/modules/pdifproperties.json -> src/main/deploy/swerve/modules/pidfproperties.json renamed: src/main/deploy/swerve/falcon/swervedrive.json -> src/main/deploy/swerve/swervedrive.json modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java modified: src/main/java/frc/robot/subsystems/vision/Vision.java modified: src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java --- .../autos/Example Auto.auto | 0 .../deploy/pathplanner/autos/New Auto.auto | 25 ++++++ .../navgrid.json | 0 .../paths/Example Path.path | 0 .../deploy/pathplanner/paths/New Path.path | 65 +++++++++++++++ .../{falcon => }/controllerproperties.json | 4 +- .../swerve/falcon/modules/backleft.json | 26 ------ .../swerve/falcon/modules/backright.json | 26 ------ .../swerve/falcon/modules/frontleft.json | 26 ------ .../swerve/falcon/modules/frontright.json | 26 ------ .../falcon/modules/physicalproperties.json | 16 ---- src/main/deploy/swerve/modules/backleft.json | 27 ++++++ src/main/deploy/swerve/modules/backright.json | 27 ++++++ src/main/deploy/swerve/modules/frontleft.json | 27 ++++++ .../deploy/swerve/modules/frontright.json | 27 ++++++ .../swerve/modules/physicalproperties.json | 23 ++++++ .../pidfproperties.json} | 6 +- .../swerve/{falcon => }/swervedrive.json | 6 +- src/main/java/frc/robot/Constants.java | 82 ++++++++++--------- src/main/java/frc/robot/RobotContainer.java | 55 ++++++++++++- .../swervedrive/SwerveSubsystem.java | 8 +- .../frc/robot/subsystems/vision/Vision.java | 51 ++++++++++-- .../subsystems/vision/VisionIOPhoton.java | 2 +- 23 files changed, 375 insertions(+), 180 deletions(-) rename src/main/deploy/{pathplanner2024 => pathplanner}/autos/Example Auto.auto (100%) create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto rename src/main/deploy/{pathplanner2024 => pathplanner}/navgrid.json (100%) rename src/main/deploy/{pathplanner2024 => pathplanner}/paths/Example Path.path (100%) create mode 100644 src/main/deploy/pathplanner/paths/New Path.path rename src/main/deploy/swerve/{falcon => }/controllerproperties.json (72%) delete mode 100644 src/main/deploy/swerve/falcon/modules/backleft.json delete mode 100644 src/main/deploy/swerve/falcon/modules/backright.json delete mode 100644 src/main/deploy/swerve/falcon/modules/frontleft.json delete mode 100644 src/main/deploy/swerve/falcon/modules/frontright.json delete mode 100644 src/main/deploy/swerve/falcon/modules/physicalproperties.json create mode 100644 src/main/deploy/swerve/modules/backleft.json create mode 100644 src/main/deploy/swerve/modules/backright.json create mode 100644 src/main/deploy/swerve/modules/frontleft.json create mode 100644 src/main/deploy/swerve/modules/frontright.json create mode 100644 src/main/deploy/swerve/modules/physicalproperties.json rename src/main/deploy/swerve/{falcon/modules/pdifproperties.json => modules/pidfproperties.json} (70%) rename src/main/deploy/swerve/{falcon => }/swervedrive.json (66%) diff --git a/src/main/deploy/pathplanner2024/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto similarity index 100% rename from src/main/deploy/pathplanner2024/autos/Example Auto.auto rename to src/main/deploy/pathplanner/autos/Example Auto.auto diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 00000000..7fc8be27 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.0, + "y": 7.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "New Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner2024/navgrid.json b/src/main/deploy/pathplanner/navgrid.json similarity index 100% rename from src/main/deploy/pathplanner2024/navgrid.json rename to src/main/deploy/pathplanner/navgrid.json diff --git a/src/main/deploy/pathplanner2024/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path similarity index 100% rename from src/main/deploy/pathplanner2024/paths/Example Path.path rename to src/main/deploy/pathplanner/paths/Example Path.path diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 00000000..2012ffc1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 6.5 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.0, + "y": 5.0 + }, + "prevControl": { + "x": 4.0, + "y": 6.0 + }, + "nextControl": { + "x": 6.0, + "y": 4.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.225280307703473, + "y": 6.012192024384049 + }, + "prevControl": { + "x": 6.975280307703473, + "y": 7.512192024384059 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/swerve/falcon/controllerproperties.json b/src/main/deploy/swerve/controllerproperties.json similarity index 72% rename from src/main/deploy/swerve/falcon/controllerproperties.json rename to src/main/deploy/swerve/controllerproperties.json index 95a19ac8..c5ab6446 100644 --- a/src/main/deploy/swerve/falcon/controllerproperties.json +++ b/src/main/deploy/swerve/controllerproperties.json @@ -1,8 +1,8 @@ { "angleJoystickRadiusDeadband": 0.5, "heading": { - "p": 0.118, + "p": 0.4, "i": 0, - "d": 0 + "d": 0.01 } } diff --git a/src/main/deploy/swerve/falcon/modules/backleft.json b/src/main/deploy/swerve/falcon/modules/backleft.json deleted file mode 100644 index fd39bc0d..00000000 --- a/src/main/deploy/swerve/falcon/modules/backleft.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "drive": { - "type": "talonfx", - "id": 4, - "canbus": null - }, - "angle": { - "type": "talonfx", - "id": 2, - "canbus": null - }, - "encoder": { - "type": "cancoder", - "id": 1, - "canbus": null - }, - "inverted": { - "drive": false, - "angle": false - }, - "absoluteEncoderOffset": -259.805, - "location": { - "front": -7, - "left": 7 - } -} diff --git a/src/main/deploy/swerve/falcon/modules/backright.json b/src/main/deploy/swerve/falcon/modules/backright.json deleted file mode 100644 index 11ac2b94..00000000 --- a/src/main/deploy/swerve/falcon/modules/backright.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "drive": { - "type": "talonfx", - "id": 3, - "canbus": null - }, - "angle": { - "type": "talonfx", - "id": 53, - "canbus": null - }, - "encoder": { - "type": "cancoder", - "id": 4, - "canbus": null - }, - "inverted": { - "drive": false, - "angle": false - }, - "absoluteEncoderOffset": -31.816, - "location": { - "front": -7, - "left": -7 - } -} diff --git a/src/main/deploy/swerve/falcon/modules/frontleft.json b/src/main/deploy/swerve/falcon/modules/frontleft.json deleted file mode 100644 index c6abe46e..00000000 --- a/src/main/deploy/swerve/falcon/modules/frontleft.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "drive": { - "type": "talonfx", - "id": 50, - "canbus": null - }, - "angle": { - "type": "talonfx", - "id": 8, - "canbus": null - }, - "encoder": { - "type": "cancoder", - "id": 3, - "canbus": null - }, - "inverted": { - "drive": false, - "angle": false - }, - "absoluteEncoderOffset": -139.043, - "location": { - "front": 7, - "left": 7 - } -} diff --git a/src/main/deploy/swerve/falcon/modules/frontright.json b/src/main/deploy/swerve/falcon/modules/frontright.json deleted file mode 100644 index ba24422c..00000000 --- a/src/main/deploy/swerve/falcon/modules/frontright.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "drive": { - "type": "talonfx", - "id": 51, - "canbus": null - }, - "angle": { - "type": "talonfx", - "id": 52, - "canbus": null - }, - "encoder": { - "type": "cancoder", - "id": 2, - "canbus": null - }, - "inverted": { - "drive": false, - "angle": false - }, - "absoluteEncoderOffset": -118.564, - "location": { - "front": 7, - "left": -7 - } -} diff --git a/src/main/deploy/swerve/falcon/modules/physicalproperties.json b/src/main/deploy/swerve/falcon/modules/physicalproperties.json deleted file mode 100644 index a0a7b127..00000000 --- a/src/main/deploy/swerve/falcon/modules/physicalproperties.json +++ /dev/null @@ -1,16 +0,0 @@ -{ - "conversionFactor": { - "angle": 28.125, - "drive": 0.047286787200699704 - }, - "currentLimit": { - "drive": 40, - "angle": 20 - }, - "rampRate": { - "drive": 0.25, - "angle": 0.25 - }, - "wheelGripCoefficientOfFriction": 1.19, - "optimalVoltage": 12 -} diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json new file mode 100644 index 00000000..614c45a4 --- /dev/null +++ b/src/main/deploy/swerve/modules/backleft.json @@ -0,0 +1,27 @@ +{ + "location": { + "front": -10.375, + "left": 10.375 + }, + "absoluteEncoderOffset": 0, + "drive": { + "type": "falcon", + "id": 7, + "canbus": "CANivore" + }, + "inverted": { + "drive": true, + "angle": true + }, + "angle": { + "type": "falcon", + "id": 8, + "canbus": "CANivore" + }, + "encoder": { + "type": "cancoder", + "id": 9, + "canbus": "CANivore" + }, + "absoluteEncoderInverted": false +} diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json new file mode 100644 index 00000000..7a8eee6f --- /dev/null +++ b/src/main/deploy/swerve/modules/backright.json @@ -0,0 +1,27 @@ +{ + "location": { + "front": -10.375, + "left": -10.375 + }, + "absoluteEncoderOffset": 0, + "drive": { + "type": "falcon", + "id": 10, + "canbus": "CANivore" + }, + "inverted": { + "drive": true, + "angle": true + }, + "angle": { + "type": "falcon", + "id": 11, + "canbus": "CANivore" + }, + "encoder": { + "type": "cancoder", + "id": 12, + "canbus": "CANivore" + }, + "absoluteEncoderInverted": false +} diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json new file mode 100644 index 00000000..d7e7cbcc --- /dev/null +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -0,0 +1,27 @@ +{ + "location": { + "front": 10.375, + "left": 10.375 + }, + "absoluteEncoderOffset": 0, + "drive": { + "type": "falcon", + "id": 1, + "canbus": "CANivore" + }, + "inverted": { + "drive": true, + "angle": true + }, + "angle": { + "type": "falcon", + "id": 2, + "canbus": "CANivore" + }, + "encoder": { + "type": "cancoder", + "id": 3, + "canbus": "CANivore" + }, + "absoluteEncoderInverted": false +} diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json new file mode 100644 index 00000000..e7f49d41 --- /dev/null +++ b/src/main/deploy/swerve/modules/frontright.json @@ -0,0 +1,27 @@ +{ + "location": { + "front": 10.375, + "left": -10.375 + }, + "absoluteEncoderOffset": 0, + "drive": { + "type": "falcon", + "id": 4, + "canbus": "CANivore" + }, + "inverted": { + "drive": true, + "angle": true + }, + "angle": { + "type": "falcon", + "id": 5, + "canbus": "CANivore" + }, + "encoder": { + "type": "cancoder", + "id": 6, + "canbus": "CANivore" + }, + "absoluteEncoderInverted": false +} diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json new file mode 100644 index 00000000..d43b4d62 --- /dev/null +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -0,0 +1,23 @@ +{ + "optimalVoltage": 12, + "wheelGripCoefficientOfFriction": 1.19, + "currentLimit": { + "drive": 40, + "angle": 20 + }, + "conversionFactors": { + "angle": { + "gearRatio": 21.42857, + "factor": 0 + }, + "drive": { + "diameter": 4, + "gearRatio": 6.74603, + "factor": 0 + } + }, + "rampRate": { + "drive": 0.25, + "angle": 0.25 + } +} diff --git a/src/main/deploy/swerve/falcon/modules/pdifproperties.json b/src/main/deploy/swerve/modules/pidfproperties.json similarity index 70% rename from src/main/deploy/swerve/falcon/modules/pdifproperties.json rename to src/main/deploy/swerve/modules/pidfproperties.json index 75d55289..b4650e38 100644 --- a/src/main/deploy/swerve/falcon/modules/pdifproperties.json +++ b/src/main/deploy/swerve/modules/pidfproperties.json @@ -1,15 +1,15 @@ { "drive": { - "p": 1, + "p": 0.0020645, "i": 0, "d": 0, "f": 0, "iz": 0 }, "angle": { - "p": 50, + "p": 0.0020645, "i": 0, - "d": 0.32, + "d": 0, "f": 0, "iz": 0 } diff --git a/src/main/deploy/swerve/falcon/swervedrive.json b/src/main/deploy/swerve/swervedrive.json similarity index 66% rename from src/main/deploy/swerve/falcon/swervedrive.json rename to src/main/deploy/swerve/swervedrive.json index 509063d9..2232e85f 100644 --- a/src/main/deploy/swerve/falcon/swervedrive.json +++ b/src/main/deploy/swerve/swervedrive.json @@ -1,10 +1,10 @@ { "imu": { - "type": "navx", + "type": "pigeon2", "id": 13, - "canbus": "canivore" + "canbus": "CANivore" }, - "invertedIMU": true, + "invertedIMU": false, "modules": [ "frontleft.json", "frontright.json", diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4b517aa8..d1cb2cf3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -49,6 +49,8 @@ public final class Constants { * Define the various multiple robots that use this same code (e.g., COMPBOT, DEVBOT, SIMBOT, * etc.) and the operating modes of the code (REAL, SIM, or REPLAY) */ + private static RobotType robotType = RobotType.SIMBOT; + public static boolean disableHAL = false; /** Enumerate the robot types (add additional bots here) */ @@ -61,8 +63,8 @@ public static enum RobotType { /** Enumerate the robot operation modes */ public static enum Mode { REAL, // REAL == Running on a real robot - SIM, // SIM == Running a physics simulator - REPLAY // REPLAY == Replaying from a log file + REPLAY, // REPLAY == Replaying from a log file + SIM // SIM == Running a physics simulator } /** Get the current robot */ @@ -75,7 +77,7 @@ public static RobotType getRobot() { return robotType; } - /** Get the current mode for robot operation, based on robot type */ + /** Get the current mode */ public static Mode getMode() { return switch (robotType) { case DEVBOT, COMPBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; @@ -100,9 +102,8 @@ public static void main(String... args) { /* The remainder of this file contains physical and/or software constants for the various subsystems of the robot */ /** General Constants **************************************************** */ - private static RobotType robotType = getRobot(); - public static final double loopPeriodSecs = 0.02; + public static final boolean tuningMode = false; /** Physical Constants for Robot Operation ************ */ @@ -160,41 +161,48 @@ public static class OperatorConstants { /** AprilTag Field Layout ************************************************ */ /* SEASON SPECIFIC! */ - public static final AprilTagFieldLayout aprilTagFieldLayout = - AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - - @Getter - public enum AprilTagLayoutType { - OFFICIAL("2024-official"), - SPEAKERS_ONLY("2024-speakers"), - AMPS_ONLY("2024-amps"), - WPI("2024-wpi"); - - private AprilTagLayoutType(String name) { - if (Constants.disableHAL) { - layout = null; - } else { - try { - layout = - new AprilTagFieldLayout( - Path.of(Filesystem.getDeployDirectory().getPath(), "apriltags", name + ".json")); - } catch (IOException e) { - throw new RuntimeException(e); + public static class AprilTagConstants { + + public static final double aprilTagWidth = Units.inchesToMeters(6.50); + public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; + + public static final AprilTagFieldLayout aprilTagFieldLayout = + AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + + @Getter + public enum AprilTagLayoutType { + OFFICIAL("2024-official"), + SPEAKERS_ONLY("2024-speakers"), + AMPS_ONLY("2024-amps"), + WPI("2024-wpi"); + + private AprilTagLayoutType(String name) { + if (Constants.disableHAL) { + layout = null; + } else { + try { + layout = + new AprilTagFieldLayout( + Path.of( + Filesystem.getDeployDirectory().getPath(), "apriltags", name + ".json")); + } catch (IOException e) { + throw new RuntimeException(e); + } } - } - if (layout == null) { - layoutString = ""; - } else { - try { - layoutString = new ObjectMapper().writeValueAsString(layout); - } catch (JsonProcessingException e) { - throw new RuntimeException( - "Failed to serialize AprilTag layout JSON " + toString() + "for Northstar"); + if (layout == null) { + layoutString = ""; + } else { + try { + layoutString = new ObjectMapper().writeValueAsString(layout); + } catch (JsonProcessingException e) { + throw new RuntimeException( + "Failed to serialize AprilTag layout JSON " + toString() + "for Northstar"); + } } } - } - private final AprilTagFieldLayout layout; - private final String layoutString; + private final AprilTagFieldLayout layout; + private final String layoutString; + } } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2f89da45..2ecfa299 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,10 +26,20 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.AprilTagConstants; +import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; import frc.robot.Constants.OperatorConstants; import frc.robot.commands.swervedrive.drivebase.AbsoluteDriveAdv; +import frc.robot.subsystems.flywheel_example.Flywheel; +import frc.robot.subsystems.flywheel_example.FlywheelIO; +import frc.robot.subsystems.flywheel_example.FlywheelIOSim; +import frc.robot.subsystems.flywheel_example.FlywheelIOTalonFX; import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.subsystems.vision.Vision; +import frc.robot.subsystems.vision.VisionIO; +import frc.robot.subsystems.vision.VisionIOPhoton; import frc.robot.util.CanDeviceId; +import frc.robot.util.OverrideSwitches; import java.io.File; public class RobotContainer { @@ -38,14 +48,53 @@ public class RobotContainer { // Replace with CommandPS4Controller or CommandJoystick if needed final CommandXboxController driverXbox = new CommandXboxController(0); final CommandXboxController operatorXbox = new CommandXboxController(1); + final OverrideSwitches overrides = new OverrideSwitches(2); - // The robot's subsystems and commands are defined here... - private final SwerveSubsystem m_drivebase = - new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo")); + // Declare the robot subsystems here + private final SwerveSubsystem m_drivebase; + private final Flywheel m_flywheel; + private final Vision m_vision; + + /** Returns the current AprilTag layout type. */ + public AprilTagLayoutType getAprilTagLayoutType() { + return AprilTagConstants.defaultAprilTagType; + } + + public static AprilTagLayoutType staticGetAprilTagLayoutType() { + return AprilTagConstants.defaultAprilTagType; + } /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + // Instantiate Robot Subsystems based on RobotType + switch (Constants.getMode()) { + case REAL: + // Real robot, instantiate hardware IO implementations + // YAGSL drivebase, get config from deploy directory + m_drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); + m_flywheel = new Flywheel(new FlywheelIOTalonFX()); + m_vision = + new Vision( + this::getAprilTagLayoutType, + new VisionIOPhoton(this::getAprilTagLayoutType, "Photon_CAMNAME")); + break; + + case SIM: + // Sim robot, instantiate physics sim IO implementations + m_drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); + m_flywheel = new Flywheel(new FlywheelIOSim()); + m_vision = new Vision(this::getAprilTagLayoutType); + break; + + default: + // Replayed robot, disable IO implementations + m_drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); + m_flywheel = new Flywheel(new FlywheelIO() {}); + m_vision = new Vision(this::getAprilTagLayoutType, new VisionIO() {}, new VisionIO() {}); + break; + } + // Configure the trigger bindings configureBindings(); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 65960442..a2134036 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -39,7 +39,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import frc.robot.Constants; +import frc.robot.Constants.AprilTagConstants; import frc.robot.Constants.AutonConstants; import frc.robot.Constants.PhysicalConstants; import frc.robot.subsystems.vision.Vision; @@ -565,7 +565,8 @@ public void addFakeVisionReading() { public double getDistanceToSpeaker() { int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; // Taken from PhotonUtils.getDistanceToPose - Pose3d speakerAprilTagPose = Constants.aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); + Pose3d speakerAprilTagPose = + AprilTagConstants.aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); return getPose().getTranslation().getDistance(speakerAprilTagPose.toPose2d().getTranslation()); } @@ -577,7 +578,8 @@ public double getDistanceToSpeaker() { public Rotation2d getSpeakerYaw() { int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; // Taken from PhotonUtils.getYawToPose() - Pose3d speakerAprilTagPose = Constants.aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); + Pose3d speakerAprilTagPose = + AprilTagConstants.aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); Translation2d relativeTrl = speakerAprilTagPose.toPose2d().relativeTo(getPose()).getTranslation(); return new Rotation2d(relativeTrl.getX(), relativeTrl.getY()) diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 7aa450e9..2ac10d55 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -32,11 +32,16 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import frc.robot.Constants; +import frc.robot.Constants.AprilTagConstants; +import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; import frc.robot.Robot; +import frc.robot.RobotContainer; +import frc.robot.subsystems.vision.VisionIO.VisionIOInputs; import java.awt.Desktop; import java.util.ArrayList; +import java.util.HashMap; import java.util.List; +import java.util.Map; import java.util.Optional; import java.util.function.Supplier; import org.photonvision.EstimatedRobotPose; @@ -75,6 +80,14 @@ public class Vision { /** Field from {@link swervelib.SwerveDrive#field} */ private Field2d field2d; + // Tag sources and layout + private final Supplier aprilTagTypeSupplier; + private final VisionIO[] io; + private final VisionIOInputs[] inputs; + // Bookkeeping for last frame exposure and last tag detection + private final Map lastFrameTimes = new HashMap<>(); + private final Map lastTagDetectionTimes = new HashMap<>(); + /** * Constructor for the Vision class. * @@ -87,7 +100,7 @@ public Vision(Supplier currentPose, Field2d field) { if (Robot.isSimulation()) { visionSim = new VisionSystemSim("Vision"); - visionSim.addAprilTags(Constants.aprilTagFieldLayout); + visionSim.addAprilTags(AprilTagConstants.aprilTagFieldLayout); for (Cameras c : Cameras.values()) { c.addToVisionSim(visionSim); @@ -95,6 +108,25 @@ public Vision(Supplier currentPose, Field2d field) { openSimCameraViews(); } + this.aprilTagTypeSupplier = RobotContainer::staticGetAprilTagLayoutType; + this.io = null; + inputs = null; + } + + // Class method definition, including inputs + // TODO: Constructor from Euclid + public Vision(Supplier aprilTagTypeSupplier, VisionIO... io) { + this.aprilTagTypeSupplier = aprilTagTypeSupplier; + this.io = io; + inputs = new VisionIOInputs[io.length]; + for (int i = 0; i < io.length; i++) { + inputs[i] = new VisionIOInputs(); + } + + // Create map of last frame times for instances + for (int i = 0; i < io.length; i++) { + lastFrameTimes.put(i, 0.0); + } } /** @@ -106,7 +138,7 @@ public Vision(Supplier currentPose, Field2d field) { * @return The target pose of the AprilTag. */ public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) { - Optional aprilTagPose3d = Constants.aprilTagFieldLayout.getTagPose(aprilTag); + Optional aprilTagPose3d = AprilTagConstants.aprilTagFieldLayout.getTagPose(aprilTag); if (aprilTagPose3d.isPresent()) { return aprilTagPose3d.get().toPose2d().transformBy(robotOffset); } else { @@ -114,7 +146,7 @@ public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) { "Cannot get AprilTag " + aprilTag + " from field " - + Constants.aprilTagFieldLayout.toString()); + + AprilTagConstants.aprilTagFieldLayout.toString()); } } @@ -261,7 +293,7 @@ public PhotonPipelineResult getLatestResult(Cameras camera) { * @return Distance */ public double getDistanceFromAprilTag(int id) { - Optional tag = Constants.aprilTagFieldLayout.getTagPose(id); + Optional tag = AprilTagConstants.aprilTagFieldLayout.getTagPose(id); return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())) .orElse(-1.0); } @@ -325,9 +357,12 @@ public void updateVisionField() { List poses = new ArrayList<>(); for (PhotonTrackedTarget target : targets) { - if (Constants.aprilTagFieldLayout.getTagPose(target.getFiducialId()).isPresent()) { + if (AprilTagConstants.aprilTagFieldLayout.getTagPose(target.getFiducialId()).isPresent()) { Pose2d targetPose = - Constants.aprilTagFieldLayout.getTagPose(target.getFiducialId()).get().toPose2d(); + AprilTagConstants.aprilTagFieldLayout + .getTagPose(target.getFiducialId()) + .get() + .toPose2d(); poses.add(targetPose); } } @@ -412,7 +447,7 @@ enum Cameras { poseEstimator = new PhotonPoseEstimator( - Constants.aprilTagFieldLayout, + AprilTagConstants.aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera, robotToCamTransform); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java index f51e9273..caa72314 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java @@ -15,7 +15,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.Timer; -import frc.robot.Constants.AprilTagLayoutType; +import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; import frc.robot.util.Alert; import java.util.List; import java.util.function.Supplier; From 503ae730751af090113bf0431571be06b732c5e4 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Tue, 15 Oct 2024 08:39:08 -0700 Subject: [PATCH 20/57] Add LF setting to vscode modified: .vscode/settings.json modified: src/main/java/frc/robot/RobotContainer.java --- .vscode/settings.json | 3 ++- src/main/java/frc/robot/RobotContainer.java | 29 ++++++++++++++++++++- 2 files changed, 30 insertions(+), 2 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 4b971ad8..c4144ddf 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -29,5 +29,6 @@ null ], "java.test.defaultConfig": "WPIlibUnitTests", - "editor.indentSize": 2 + "editor.indentSize": 2, + "files.eol": "\n" } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2ecfa299..7d677cc7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,8 @@ package frc.robot; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -24,6 +26,7 @@ import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.AprilTagConstants; @@ -41,6 +44,7 @@ import frc.robot.util.CanDeviceId; import frc.robot.util.OverrideSwitches; import java.io.File; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class RobotContainer { @@ -55,6 +59,9 @@ public class RobotContainer { private final Flywheel m_flywheel; private final Vision m_vision; + // Dashboard inputs + private final LoggedDashboardChooser autoChooser; + /** Returns the current AprilTag layout type. */ public AprilTagLayoutType getAprilTagLayoutType() { return AprilTagConstants.defaultAprilTagType; @@ -97,7 +104,16 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); + // Define TeleOp commands + defineTeleopCommands(); + // Define Auto commands + defineAutoCommands(); + // Set up the SmartDashboard Auto Chooser + autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); + } + /** Use this method to define your TeleOp commands. */ + private void defineTeleopCommands() { // Applies deadbands and inverts controls because joysticks // are back-right positive while robot // controls are front-left positive @@ -152,6 +168,12 @@ public RobotContainer() { : driveFieldOrientedDirectAngleSim); } + /** Use this method to define your Autonomous commands. */ + private void defineAutoCommands() { + NamedCommands.registerCommand( + "Zero", Commands.runOnce(() -> m_drivebase.zeroGyroWithAlliance())); + } + /** * Use this method to define your trigger->command mappings. Triggers can be created via the * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary @@ -161,7 +183,10 @@ public RobotContainer() { * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight * joysticks}. */ - private void configureBindings() {} + private void configureBindings() { + // Manually Re-Zero the Gyro + driverXbox.y().onTrue(Commands.runOnce(() -> m_drivebase.zeroGyroWithAlliance())); + } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -171,6 +196,8 @@ private void configureBindings() {} public Command getAutonomousCommand() { // An example command will be run in autonomous return m_drivebase.getAutonomousCommand("New Auto"); + // Use the ``autoChooser`` to define your auto path from the SmartDashboard + // return autoChooser.get(); } public void setDriveMode() { From 9a1348f41abe57f6eeb474ef54acd86ed4896c53 Mon Sep 17 00:00:00 2001 From: Lewis Ruskin Date: Wed, 16 Oct 2024 18:10:24 -0700 Subject: [PATCH 21/57] Added default GitHub action --- .github/workflows/main.yml | 38 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100644 .github/workflows/main.yml diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml new file mode 100644 index 00000000..f07d3241 --- /dev/null +++ b/.github/workflows/main.yml @@ -0,0 +1,38 @@ +# This is a basic workflow to build robot code. + +name: CI + +# Controls when the action will run. Triggers the workflow on push or pull request +# events but only for the main branch. +on: + push: + branches: [ main ] + pull_request: + branches: [ main ] + +# A workflow run is made up of one or more jobs that can run sequentially or in parallel +jobs: + # This workflow contains a single job called "build" + build: + # The type of runner that the job will run on + runs-on: ubuntu-latest + + # This grabs the WPILib docker container + container: wpilib/roborio-cross-ubuntu:2024-22.04 + + # Steps represent a sequence of tasks that will be executed as part of the job + steps: + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v4 + + # Declares the repository safe and not under dubious ownership. + - name: Add repository to git safe directories + run: git config --global --add safe.directory $GITHUB_WORKSPACE + + # Grant execute permission for gradlew + - name: Grant execute permission for gradlew + run: chmod +x gradlew + + # Runs a single command using the runners shell + - name: Compile and run tests on robot code + run: ./gradlew build From 8bfacdfafa3e25c42517cca69ab4ce936d96eaca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9CLewis?= <“lew.ruskin”@gmail.com”> Date: Wed, 16 Oct 2024 18:15:55 -0700 Subject: [PATCH 22/57] added status badge to readme --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 7be4194f..448d7786 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,6 @@ +[![CI](https://github.com/AZ-First/Az-RBSI/actions/workflows/main.yml/badge.svg)](https://github.com/AZ-First/Az-RBSI/actions/workflows/main.yml) + + ![AzFIRST Logo](https://github.com/AZ-First/Az-RBSI/blob/main/AZ-First-logo.png?raw=true) # Az-RBSI From 4a387ba50e9e0fde38a84ad5a5bf845dd228ba55 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9CLewis?= <“lew.ruskin”@gmail.com”> Date: Wed, 16 Oct 2024 19:26:07 -0700 Subject: [PATCH 23/57] added spotless for code cleanliness --- .github/workflows/main.yml | 19 +++++++++++++++++- .wpilib/wpilib_preferences.json | 3 +-- build.gradle | 35 +++++++++++++++++++-------------- 3 files changed, 39 insertions(+), 18 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index f07d3241..5a4c3a0a 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -12,7 +12,7 @@ on: # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: - # This workflow contains a single job called "build" + # This workflow contains a job called "build" which is the main build build: # The type of runner that the job will run on runs-on: ubuntu-latest @@ -36,3 +36,20 @@ jobs: # Runs a single command using the runners shell - name: Compile and run tests on robot code run: ./gradlew build + - run: ./gradlew spotlessCheck + + # This workflow contains a job called "build" which is the main build + spotless: + # The type of runner that the job will run on + runs-on: ubuntu-latest + # Steps represent a sequence of tasks that will be executed as part of the job + steps: + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + - uses: actions/setup-java@v4 + with: + distribution: 'zulu' + java-version: 17 + - run: ./gradlew spotlessCheck \ No newline at end of file diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 319f962f..c93b4209 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,5 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2024", - "teamNumber": 2486 + "projectYear": "2024" } diff --git a/build.gradle b/build.gradle index 8b0da398..9042c8c2 100644 --- a/build.gradle +++ b/build.gradle @@ -152,11 +152,13 @@ gversion { // Spotless formatting project.compileJava.dependsOn(spotlessApply) spotless { + enforceCheck true java { - target fileTree(".") { - include "**/*.java" - exclude "**/build/**", "**/build-*/**" + target fileTree('.') { + include '**/*.java' + exclude '**/build/**', '**/build-*/**' } + importOrder() toggleOffOn() googleJavaFormat() removeUnusedImports() @@ -164,26 +166,29 @@ spotless { endWithNewline() } groovyGradle { - target fileTree(".") { - include "**/*.gradle" - exclude "**/build/**", "**/build-*/**" + target fileTree('.') { + include '**/*.gradle' + exclude '**/build/**', '**/build-*/**' } greclipse() indentWithSpaces(4) trimTrailingWhitespace() endWithNewline() } - json { - target fileTree(".") { - include "**/*.json" - exclude "**/build/**", "**/build-*/**" + format 'xml', { + target fileTree('.') { + include '**/*.xml' + exclude '**/build/**', '**/build-*/**' } - gson().indentWithSpaces(2) + eclipseWtp('xml') + trimTrailingWhitespace() + indentWithSpaces(2) + endWithNewline() } - format "misc", { - target fileTree(".") { - include "**/*.md", "**/.gitignore" - exclude "**/build/**", "**/build-*/**" + format 'misc', { + target fileTree('.') { + include '**/*.md', '**/.gitignore' + exclude '**/build/**', '**/build-*/**' } trimTrailingWhitespace() indentWithSpaces(2) From 4068eb7f733185707314a2fd9d6027d9e187655f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9CLewis?= <“lew.ruskin”@gmail.com”> Date: Wed, 16 Oct 2024 19:46:31 -0700 Subject: [PATCH 24/57] sorts imports + imports sorted --- README.md | 2 +- build.gradle | 11 +++++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 448d7786..333df17c 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -[![CI](https://github.com/AZ-First/Az-RBSI/actions/workflows/main.yml/badge.svg)](https://github.com/AZ-First/Az-RBSI/actions/workflows/main.yml) +[![CI](https://github.com/AZ-First/Az-RBSI/actions/workflows/main.yml/badge.svg)](https://github.com/AZ-First/Az-RBSI/actions/workflows/main.yml) ![AzFIRST Logo](https://github.com/AZ-First/Az-RBSI/blob/main/AZ-First-logo.png?raw=true) diff --git a/build.gradle b/build.gradle index 8b0da398..ada890ac 100644 --- a/build.gradle +++ b/build.gradle @@ -160,6 +160,7 @@ spotless { toggleOffOn() googleJavaFormat() removeUnusedImports() + importOrder() trimTrailingWhitespace() endWithNewline() } @@ -180,6 +181,16 @@ spotless { } gson().indentWithSpaces(2) } + format 'xml', { + target fileTree('.') { + include '**/*.xml' + exclude '**/build/**', '**/build-*/**' + } + eclipseWtp('xml') + trimTrailingWhitespace() + indentWithSpaces(2) + endWithNewline() + } format "misc", { target fileTree(".") { include "**/*.md", "**/.gitignore" From 0f30ad4549361fb0df9dc1c80241b007f0cd3654 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Tue, 15 Oct 2024 08:39:08 -0700 Subject: [PATCH 25/57] Minor tweaks Hopefully fix main.yaml crash; add teamNumber=0 and gitignore the wpilib_preferences.json file. modified: .github/workflows/main.yml modified: .gitignore modified: .vscode/settings.json modified: .wpilib/wpilib_preferences.json modified: src/main/java/frc/robot/RobotContainer.java --- .github/workflows/main.yml | 1 - .gitignore | 5 ++++- .vscode/settings.json | 1 + .wpilib/wpilib_preferences.json | 3 ++- src/main/java/frc/robot/RobotContainer.java | 8 +++++++- 5 files changed, 14 insertions(+), 4 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 5a4c3a0a..af1c2e1b 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -36,7 +36,6 @@ jobs: # Runs a single command using the runners shell - name: Compile and run tests on robot code run: ./gradlew build - - run: ./gradlew spotlessCheck # This workflow contains a job called "build" which is the main build spotless: diff --git a/.gitignore b/.gitignore index b8b5f586..e144021e 100644 --- a/.gitignore +++ b/.gitignore @@ -173,4 +173,7 @@ out/ simgui*.json # Version file -src/main/java/frc/robot/BuildConstants.java \ No newline at end of file +src/main/java/frc/robot/BuildConstants.java + +# Do not push changes to the wpilib_preferences,json file +wpilib_preferences.json diff --git a/.vscode/settings.json b/.vscode/settings.json index c4144ddf..167548c9 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -30,5 +30,6 @@ ], "java.test.defaultConfig": "WPIlibUnitTests", "editor.indentSize": 2, + "editor.tabSize": 2, "files.eol": "\n" } diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index c93b4209..d482a150 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,5 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2024" + "projectYear": "2024", + "teamNumber": 0 } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7d677cc7..933a91f2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -114,6 +114,7 @@ public RobotContainer() { /** Use this method to define your TeleOp commands. */ private void defineTeleopCommands() { + // Applies deadbands and inverts controls because joysticks // are back-right positive while robot // controls are front-left positive @@ -168,8 +169,9 @@ private void defineTeleopCommands() { : driveFieldOrientedDirectAngleSim); } - /** Use this method to define your Autonomous commands. */ + /** Use this method to define your Autonomous commands for use with PathPlanner / Choreo */ private void defineAutoCommands() { + NamedCommands.registerCommand( "Zero", Commands.runOnce(() -> m_drivebase.zeroGyroWithAlliance())); } @@ -184,6 +186,7 @@ private void defineAutoCommands() { * joysticks}. */ private void configureBindings() { + // Manually Re-Zero the Gyro driverXbox.y().onTrue(Commands.runOnce(() -> m_drivebase.zeroGyroWithAlliance())); } @@ -194,6 +197,7 @@ private void configureBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { + // An example command will be run in autonomous return m_drivebase.getAutonomousCommand("New Auto"); // Use the ``autoChooser`` to define your auto path from the SmartDashboard @@ -201,10 +205,12 @@ public Command getAutonomousCommand() { } public void setDriveMode() { + // drivebase.setDefaultCommand(); } public void setMotorBrake(boolean brake) { + m_drivebase.setMotorBrake(brake); } From 0ede27247a4fd4b936f4f31937f509ee646b89f4 Mon Sep 17 00:00:00 2001 From: Lewis Ruskin Date: Thu, 17 Oct 2024 12:24:28 -0700 Subject: [PATCH 26/57] added json section to linter --- build.gradle | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/build.gradle b/build.gradle index 0e8a6c46..68225681 100644 --- a/build.gradle +++ b/build.gradle @@ -175,6 +175,13 @@ spotless { trimTrailingWhitespace() endWithNewline() } + json { + target fileTree('.'){ + include '**/*.json' + exclude '**/build/**', '**/build-*/**' + } + gson().indentWithSpaces(2) + } format 'xml', { target fileTree('.') { include '**/*.xml' From d483e428afeb803d0b69b95dff8f432f0472f486 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Mon, 21 Oct 2024 14:04:21 -0700 Subject: [PATCH 27/57] First pass through the swerve drive subsystem Moving things around, moving constants to Constants, adding various logging that AdvantageKit sketches usually produces. Also, modify the ``flywheel_example`` to conform to some of the other changes being made. Next step: see about implementing the CTRE Pro functionality, if licensed. modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java modified: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java modified: src/main/java/frc/robot/subsystems/vision/Vision.java --- src/main/java/frc/robot/Constants.java | 56 ++- src/main/java/frc/robot/RobotContainer.java | 3 +- .../subsystems/flywheel_example/Flywheel.java | 11 +- .../flywheel_example/FlywheelIOSparkMax.java | 11 +- .../flywheel_example/FlywheelIOTalonFX.java | 11 +- .../swervedrive/SwerveSubsystem.java | 386 ++++++++++-------- .../frc/robot/subsystems/vision/Vision.java | 36 ++ 7 files changed, 330 insertions(+), 184 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d1cb2cf3..1afa3257 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -112,8 +112,6 @@ public static final class PhysicalConstants { public static final Matter kChassis = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), PhysicalConstants.kRobotMass); public static final double kLoopTime = 0.13; // s, 20ms + 110ms sprk max velocity lag - public static final double kMaxSpeed = Units.feetToMeters(14.5); - // Maximum speed of the robot in meters per second, used to limit acceleration. } /** Power Distribution Module Constants ********************************** */ @@ -138,15 +136,69 @@ public static final class PowerConstants { /** Autonomous Action Constants ****************************************** */ public static final class AutonConstants { + + // Translation PID constants public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0); + // Rotation PID constants public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01); } /** Drive Base Constants ************************************************* */ public static final class DrivebaseConstants { + // Physical size of the drive base + private static final double TRACK_WIDTH_X = Units.inchesToMeters(20.75); + private static final double TRACK_WIDTH_Y = Units.inchesToMeters(20.75); + public static final double DRIVE_BASE_RADIUS = + Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); + + // Maximum chassis speeds desired for robot motion -- metric / radians + public static final double MAX_LINEAR_SPEED = Units.feetToMeters(18); // ft/s + public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; + // Maximum chassis accelerations desired for robot motion -- metric / radians + public static final double MAX_LINEAR_ACCEL = 4.0; // m/s/s + public static final double MAX_ANGULAR_ACCEL = Units.degreesToRadians(720); // deg/s/s + + // Wheel radius + public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); + + // ** Gear ratios for SDS MK4i L2, adjust as necessary ** + public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + public static final double TURN_GEAR_RATIO = 150.0 / 7.0; + // Hold time on motor brakes when disabled public static final double WHEEL_LOCK_TIME = 10; // seconds + + // SysID characterization constants + public static final double kMaxV = 12.0; // Max volts + public static final double kDelay = 3.0; // seconds + public static final double kQuasiTimeout = 5.0; // seconds + public static final double kDynamicTimeout = 3.0; // seconds + } + + /** Example Flywheel Mechanism Constants ********************************* */ + public static final class FlywheelConstants { + + // Mechanism motor gear ratio + public static final double GEAR_RATIO = 1.5; + + // MODE == REAL / REPLAY + // Feedforward constants + public static final double kStaticGainReal = 0.1; + public static final double kVelocityGainReal = 0.05; + // Feedback (PID) constants + public static final double kPReal = 1.0; + public static final double kIReal = 0.0; + public static final double kDReal = 0.0; + + // MODE == SIM + // Feedforward constants + public static final double kStaticGainSim = 0.0; + public static final double kVelocityGainSim = 0.03; + // Feedback (PID) constants + public static final double kPSim = 1.0; + public static final double kISim = 0.0; + public static final double kDSim = 0.0; } /** Operator Constants *************************************************** */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 933a91f2..e9325125 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -248,7 +248,8 @@ public static class Ports { /* SUBSYSTEM CAN DEVICE IDS */ // This is where mechanism subsystem devices are defined // Example: - // public static final CanDeviceId ELEVATOR_MAIN = new CanDeviceId(3, ""); + public static final CanDeviceId FLYWHEEL_LEADER = new CanDeviceId(3, ""); + public static final CanDeviceId FLYWHEEL_FOLLOWER = new CanDeviceId(4, ""); /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */ // This is where digital I/O feedback devices are defined diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java index 325bc9e3..db24d82c 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java @@ -14,6 +14,7 @@ package frc.robot.subsystems.flywheel_example; import static edu.wpi.first.units.Units.*; +import static frc.robot.Constants.FlywheelConstants.*; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; @@ -39,12 +40,12 @@ public Flywheel(FlywheelIO io) { switch (Constants.getMode()) { case REAL: case REPLAY: - ffModel = new SimpleMotorFeedforward(0.1, 0.05); - io.configurePID(1.0, 0.0, 0.0); + ffModel = new SimpleMotorFeedforward(kStaticGainReal, kVelocityGainReal); + io.configurePID(kPReal, kIReal, kDReal); break; case SIM: - ffModel = new SimpleMotorFeedforward(0.0, 0.03); - io.configurePID(0.5, 0.0, 0.0); + ffModel = new SimpleMotorFeedforward(kStaticGainSim, kVelocityGainSim); + io.configurePID(kPSim, kISim, kDSim); break; default: ffModel = new SimpleMotorFeedforward(0.0, 0.0); @@ -98,7 +99,7 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { } /** Returns the current velocity in RPM. */ - @AutoLogOutput + @AutoLogOutput(key = "Mechanism/Flywheel") public double getVelocityRPM() { return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec); } diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java index f9990aae..5c6c9797 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java @@ -13,6 +13,8 @@ package frc.robot.subsystems.flywheel_example; +import static frc.robot.Constants.FlywheelConstants.*; + import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; @@ -20,16 +22,19 @@ import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController.ArbFFUnits; import edu.wpi.first.math.util.Units; +import frc.robot.RobotContainer.Ports; /** * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with * "CANSparkFlex". */ public class FlywheelIOSparkMax implements FlywheelIO { - private static final double GEAR_RATIO = 1.5; - private final CANSparkMax leader = new CANSparkMax(0, MotorType.kBrushless); - private final CANSparkMax follower = new CANSparkMax(1, MotorType.kBrushless); + // Define the leader / follower motors from the Ports section of RobotContainer + private final CANSparkMax leader = + new CANSparkMax(Ports.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless); + private final CANSparkMax follower = + new CANSparkMax(Ports.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless); private final RelativeEncoder encoder = leader.getEncoder(); private final SparkPIDController pid = leader.getPIDController(); diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java index 0cd34f41..b5f99f91 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java @@ -13,6 +13,8 @@ package frc.robot.subsystems.flywheel_example; +import static frc.robot.Constants.FlywheelConstants.*; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Slot0Configs; @@ -23,12 +25,15 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.util.Units; +import frc.robot.RobotContainer.Ports; public class FlywheelIOTalonFX implements FlywheelIO { - private static final double GEAR_RATIO = 1.5; - private final TalonFX leader = new TalonFX(0); - private final TalonFX follower = new TalonFX(1); + // Define the leader / follower motors from the Ports section of RobotContainer + private final TalonFX leader = + new TalonFX(Ports.FLYWHEEL_LEADER.getDeviceNumber(), Ports.FLYWHEEL_LEADER.getBus()); + private final TalonFX follower = + new TalonFX(Ports.FLYWHEEL_FOLLOWER.getDeviceNumber(), Ports.FLYWHEEL_FOLLOWER.getBus()); private final StatusSignal leaderPosition = leader.getPosition(); private final StatusSignal leaderVelocity = leader.getVelocity(); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index a2134036..1a9331bd 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -19,6 +19,8 @@ package frc.robot.subsystems.swervedrive; +import static frc.robot.Constants.DrivebaseConstants.*; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.path.PathConstraints; @@ -26,25 +28,23 @@ import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import frc.robot.Constants.AprilTagConstants; import frc.robot.Constants.AutonConstants; -import frc.robot.Constants.PhysicalConstants; import frc.robot.subsystems.vision.Vision; import java.io.File; import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; import swervelib.SwerveController; @@ -75,39 +75,44 @@ public class SwerveSubsystem extends SubsystemBase { * @param directory Directory of swerve drive config files. */ public SwerveSubsystem(File directory) { + // Angle conversion factor is 360 / (GEAR RATIO * ENCODER RESOLUTION) - // In this case the gear ratio is 12.8 motor revolutions per wheel rotation. - // The encoder resolution per motor revolution is 1 per motor revolution. - double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(12.8); + double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(TURN_GEAR_RATIO); + // Motor conversion factor is (PI * WHEEL DIAMETER IN METERS) / (GEAR RATIO * ENCODER - // RESOLUTION). - // In this case the wheel diameter is 4 inches, which must be converted to meters to get - // meters/second. - // The gear ratio is 6.75 motor revolutions per wheel rotation. - // The encoder resolution per motor revolution is 1 per motor revolution. + // RESOLUTION) double driveConversionFactor = - SwerveMath.calculateMetersPerRotation(Units.inchesToMeters(4), 6.75); + SwerveMath.calculateMetersPerRotation(WHEEL_RADIUS * 2.0, DRIVE_GEAR_RATIO); + + // Output to console and to AdvantageKit System.out.println("\"conversionFactors\": {"); System.out.println("\t\"angle\": {\"factor\": " + angleConversionFactor + " },"); System.out.println("\t\"drive\": {\"factor\": " + driveConversionFactor + " }"); System.out.println("}"); + Logger.recordOutput("SwerveDive/ConversionFactors/Angle", angleConversionFactor); + Logger.recordOutput("SwerveDive/ConversionFactors/Drive", driveConversionFactor); // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being // created. SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; try { - swerveDrive = new SwerveParser(directory).createSwerveDrive(PhysicalConstants.kMaxSpeed); + swerveDrive = new SwerveParser(directory).createSwerveDrive(MAX_LINEAR_SPEED); // Alternative method if you don't want to supply the conversion factor via JSON files. // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, // angleConversionFactor, driveConversionFactor); } catch (Exception e) { throw new RuntimeException(e); } - swerveDrive.setHeadingCorrection( - false); // Heading correction should only be used while controlling the robot via angle. - swerveDrive.setCosineCompensator( - false); // !SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for - // simulations since it causes discrepancies not seen in real life. + // Heading correction should only be used while controlling the robot via angle. + swerveDrive.setHeadingCorrection(false); + // Disables cosine compensation for simulations since it causes discrepancies not seen in real + // life. + if (SwerveDriveTelemetry.isSimulation) { + swerveDrive.setCosineCompensator(false); + } else { + swerveDrive.setCosineCompensator(true); + } + if (visionDriveTest) { setupPhotonVision(); // Stop the odometry thread if we are using vision that way we can synchronize updates better. @@ -124,7 +129,7 @@ public SwerveSubsystem(File directory) { */ public SwerveSubsystem( SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { - swerveDrive = new SwerveDrive(driveCfg, controllerCfg, PhysicalConstants.kMaxSpeed); + swerveDrive = new SwerveDrive(driveCfg, controllerCfg, MAX_LINEAR_SPEED); } /** Setup the photon vision class. */ @@ -132,40 +137,19 @@ public void setupPhotonVision() { vision = new Vision(swerveDrive::getPose, swerveDrive.field); } - @Override - public void periodic() { - // When vision is enabled we must manually update odometry in SwerveDrive - if (visionDriveTest) { - swerveDrive.updateOdometry(); - vision.updatePoseEstimation(swerveDrive); - } - } - - @Override - public void simulationPeriodic() {} - /** Setup AutoBuilder for PathPlanner. */ public void setupPathPlanner() { AutoBuilder.configureHolonomic( this::getPose, // Robot pose supplier - this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting - // pose) + this::resetOdometry, // Method to reset odometry (called if your auto has a starting pose) this::getRobotVelocity, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - this::setChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE - // ChassisSpeeds - new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in - // your Constants class + this::setChassisSpeeds, // Driver method given ROBOT RELATIVE ChassisSpeeds + new HolonomicPathFollowerConfig( AutonConstants.TRANSLATION_PID, - // Translation PID constants AutonConstants.ANGLE_PID, - // Rotation PID constants - 4.5, - // Max module speed, in m/s - swerveDrive.swerveDriveConfiguration.getDriveBaseRadiusMeters(), - // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig() - // Default path replanning config. See the API for the options here - ), + MAX_LINEAR_SPEED, + DRIVE_BASE_RADIUS, + new ReplanningConfig()), () -> { // Boolean supplier that controls when the path will be mirrored for the red alliance // This will flip the path being followed to the red side of the field. @@ -177,42 +161,90 @@ public void setupPathPlanner() { ); } + /** Periodic function -- update odometry and log everything */ + @Override + public void periodic() { + // When vision is enabled we must manually update odometry in SwerveDrive + if (visionDriveTest) { + swerveDrive.updateOdometry(); + vision.updatePoseEstimation(swerveDrive); + } + + /** Log Telemetry Data to AdvantageKit */ + Logger.recordOutput("SwerveDive/Telemetry/moduleCount", SwerveDriveTelemetry.moduleCount); + Logger.recordOutput("SwerveDive/Telemetry/wheelLocations", SwerveDriveTelemetry.wheelLocations); + Logger.recordOutput("SwerveDive/Telemetry/measuredStates", SwerveDriveTelemetry.measuredStates); + Logger.recordOutput("SwerveDive/Telemetry/desiredStates", SwerveDriveTelemetry.desiredStates); + Logger.recordOutput("SwerveDive/Telemetry/robotRotation", SwerveDriveTelemetry.robotRotation); + Logger.recordOutput("SwerveDive/Telemetry/maxSpeed", SwerveDriveTelemetry.maxSpeed); + Logger.recordOutput("SwerveDive/Telemetry/rotationUnit", SwerveDriveTelemetry.rotationUnit); + Logger.recordOutput("SwerveDive/Telemetry/sizeLeftRight", SwerveDriveTelemetry.sizeLeftRight); + Logger.recordOutput("SwerveDive/Telemetry/sizeFrontBack", SwerveDriveTelemetry.sizeFrontBack); + Logger.recordOutput( + "SwerveDive/Telemetry/forwardDirection", SwerveDriveTelemetry.forwardDirection); + Logger.recordOutput( + "SwerveDive/Telemetry/maxAngularVelocity", SwerveDriveTelemetry.maxAngularVelocity); + Logger.recordOutput( + "SwerveDive/Telemetry/measuredChassisSpeeds", SwerveDriveTelemetry.measuredChassisSpeeds); + Logger.recordOutput( + "SwerveDive/Telemetry/desiredChassisSpeeds", SwerveDriveTelemetry.desiredChassisSpeeds); + + /** Log Swerve Drive States to AdvantageKit */ + getModuleStates(); + getDesiredStates(); + Logger.recordOutput( + "SwerveDive/States/RobotRotation", + SwerveDriveTelemetry.rotationUnit == "degrees" + ? Rotation2d.fromDegrees(SwerveDriveTelemetry.robotRotation) + : Rotation2d.fromRadians(SwerveDriveTelemetry.robotRotation)); + } + + /** Simulation periodic function */ + @Override + public void simulationPeriodic() {} + /** - * Get the path follower with events. + * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation + * rate, and calculates and commands module states accordingly. Can use either open-loop or + * closed-loop velocity control for the wheel velocities. Also has field- and robot-relative + * modes, which affect how the translation vector is used. * - * @param pathName PathPlanner path name. - * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. + * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in + * meters per second. In robot-relative mode, positive x is torwards the bow (front) and + * positive y is torwards port (left). In field-relative mode, positive x is away from the + * alliance wall (field North) and positive y is torwards the left wall when looking through + * the driver station glass (field West). + * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by + * field/robot relativity. + * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. */ - public Command getAutonomousCommand(String pathName) { - // Create a path following command using AutoBuilder. This will also trigger event markers. - return new PathPlannerAuto(pathName); + public void drive(Translation2d translation, double rotation, boolean fieldRelative) { + + // Open loop is disabled since it shouldn't be used most of the time. + swerveDrive.drive(translation, rotation, fieldRelative, false); } /** - * Use PathPlanner Path finding to go to a point on the field. + * Drive the robot given a chassis field oriented velocity. * - * @param pose Target {@link Pose2d} to go to. - * @return PathFinding command + * @param velocity Velocity according to the field. */ - public Command driveToPose(Pose2d pose) { - // Create the constraints to use while pathfinding - PathConstraints constraints = - new PathConstraints( - swerveDrive.getMaximumVelocity(), - 4.0, - swerveDrive.getMaximumAngularVelocity(), - Units.degreesToRadians(720)); + public void driveFieldOriented(ChassisSpeeds velocity) { + swerveDrive.driveFieldOriented(velocity); + } - // Since AutoBuilder is configured, we can use it to build pathfinding commands - return AutoBuilder.pathfindToPose( - pose, - constraints, - 0.0, // Goal end velocity in meters/sec - 0.0 // Rotation delay distance in meters. This is how far the robot should travel before - // attempting to rotate. - ); + /** + * Drive according to the chassis robot oriented velocity. + * + * @param velocity Robot oriented {@link ChassisSpeeds} + */ + public void drive(ChassisSpeeds velocity) { + swerveDrive.drive(velocity); } + /************************************************************************* */ + /* COMMAND SECTION -- Swerve-only Commands */ + /** * Command to drive the robot using translative values and heading as a setpoint. * @@ -247,6 +279,32 @@ public Command driveCommand( }); } + /** + * Command to drive the robot using translative values and heading as angular velocity. NOTE: + * Alternate drive command constructor + * + * @param translationX Translation in the X direction. Cubed for smoother controls. + * @param translationY Translation in the Y direction. Cubed for smoother controls. + * @param angularRotationX Angular velocity of the robot to set. Cubed for smoother controls. + * @return Drive command. + */ + public Command driveCommand( + DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) { + return run( + () -> { + // Make the robot move + swerveDrive.drive( + SwerveMath.scaleTranslation( + new Translation2d( + translationX.getAsDouble() * swerveDrive.getMaximumVelocity(), + translationY.getAsDouble() * swerveDrive.getMaximumVelocity()), + 0.8), + Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumAngularVelocity(), + true, + false); + }); + } + /** * Command to drive the robot using translative values and heading as a setpoint. * @@ -273,96 +331,73 @@ public Command simDriveCommand( } /** - * Command to characterize the robot drive motors using SysId + * Get the path follower with events. * - * @return SysId Drive Command + * @param pathName PathPlanner path name. + * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. */ - public Command sysIdDriveMotorCommand() { - return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setDriveSysIdRoutine(new Config(), this, swerveDrive, 12), 3.0, 5.0, 3.0); + public Command getAutonomousCommand(String pathName) { + // Create a path following command using AutoBuilder. This will also trigger event markers. + return new PathPlannerAuto(pathName); } /** - * Command to characterize the robot angle motors using SysId + * Use PathPlanner Path finding to go to a point on the field. * - * @return SysId Angle Command + * @param pose Target {@link Pose2d} to go to. + * @return PathFinding command */ - public Command sysIdAngleMotorCommand() { - return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setAngleSysIdRoutine(new Config(), this, swerveDrive), 3.0, 5.0, 3.0); - } + public Command driveToPose(Pose2d pose) { + // Create the constraints to use while pathfinding + PathConstraints constraints = + new PathConstraints( + MAX_LINEAR_SPEED, MAX_LINEAR_ACCEL, MAX_ANGULAR_SPEED, MAX_ANGULAR_ACCEL); - /** - * Command to drive the robot using translative values and heading as angular velocity. - * - * @param translationX Translation in the X direction. Cubed for smoother controls. - * @param translationY Translation in the Y direction. Cubed for smoother controls. - * @param angularRotationX Angular velocity of the robot to set. Cubed for smoother controls. - * @return Drive command. - */ - public Command driveCommand( - DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) { - return run( - () -> { - // Make the robot move - swerveDrive.drive( - SwerveMath.scaleTranslation( - new Translation2d( - translationX.getAsDouble() * swerveDrive.getMaximumVelocity(), - translationY.getAsDouble() * swerveDrive.getMaximumVelocity()), - 0.8), - Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumAngularVelocity(), - true, - false); - }); + // Since AutoBuilder is configured, we can use it to build pathfinding commands + return AutoBuilder.pathfindToPose( + pose, + constraints, + 0.0, // Goal end velocity in meters/sec + 0.0 // Rotation delay distance in meters. This is how far the robot should travel before + // attempting to rotate. + ); } /** - * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation - * rate, and calculates and commands module states accordingly. Can use either open-loop or - * closed-loop velocity control for the wheel velocities. Also has field- and robot-relative - * modes, which affect how the translation vector is used. + * Command to characterize the robot drive motors using SysId * - * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in - * meters per second. In robot-relative mode, positive x is torwards the bow (front) and - * positive y is torwards port (left). In field-relative mode, positive x is away from the - * alliance wall (field North) and positive y is torwards the left wall when looking through - * the driver station glass (field West). - * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by - * field/robot relativity. - * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. + * @return SysId Drive Command */ - public void drive(Translation2d translation, double rotation, boolean fieldRelative) { - swerveDrive.drive( - translation, - rotation, - fieldRelative, - false); // Open loop is disabled since it shouldn't be used most of the time. + public Command sysIdDriveMotorCommand() { + return SwerveDriveTest.generateSysIdCommand( + SwerveDriveTest.setDriveSysIdRoutine(new Config(), this, swerveDrive, kMaxV), + kDelay, + kQuasiTimeout, + kDynamicTimeout); } /** - * Drive the robot given a chassis field oriented velocity. + * Command to characterize the robot angle motors using SysId * - * @param velocity Velocity according to the field. + * @return SysId Angle Command */ - public void driveFieldOriented(ChassisSpeeds velocity) { - swerveDrive.driveFieldOriented(velocity); + public Command sysIdAngleMotorCommand() { + return SwerveDriveTest.generateSysIdCommand( + SwerveDriveTest.setAngleSysIdRoutine(new Config(), this, swerveDrive), + kDelay, + kQuasiTimeout, + kDynamicTimeout); } - /** - * Drive according to the chassis robot oriented velocity. - * - * @param velocity Robot oriented {@link ChassisSpeeds} - */ - public void drive(ChassisSpeeds velocity) { - swerveDrive.drive(velocity); - } + /************************************************************************* */ + /* UTILITY SECTION -- Utility methods */ /** * Get the swerve drive kinematics object. * * @return {@link SwerveDriveKinematics} of the swerve drive. */ + @AutoLogOutput(key = "Odometry/Kinematics") public SwerveDriveKinematics getKinematics() { return swerveDrive.kinematics; } @@ -383,6 +418,7 @@ public void resetOdometry(Pose2d initialHolonomicPose) { * * @return The robot's pose */ + @AutoLogOutput(key = "Odometry/RobotPose") public Pose2d getPose() { return swerveDrive.getPose(); } @@ -453,6 +489,7 @@ public void setMotorBrake(boolean brake) { * * @return The yaw angle */ + @AutoLogOutput(key = "Odometry/Heading") public Rotation2d getHeading() { return getPose().getRotation(); } @@ -476,7 +513,7 @@ public ChassisSpeeds getTargetSpeeds( headingX, headingY, getHeading().getRadians(), - PhysicalConstants.kMaxSpeed); + MAX_LINEAR_SPEED); } /** @@ -496,7 +533,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d an scaledInputs.getY(), angle.getRadians(), getHeading().getRadians(), - PhysicalConstants.kMaxSpeed); + MAX_LINEAR_SPEED); } /** @@ -504,6 +541,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d an * * @return A ChassisSpeeds object of the current field-relative velocity */ + @AutoLogOutput(key = "Odometry/FieldVelocity") public ChassisSpeeds getFieldVelocity() { return swerveDrive.getFieldVelocity(); } @@ -513,6 +551,7 @@ public ChassisSpeeds getFieldVelocity() { * * @return A {@link ChassisSpeeds} object of the current velocity */ + @AutoLogOutput(key = "Odometry/RobotVelocity") public ChassisSpeeds getRobotVelocity() { return swerveDrive.getRobotVelocity(); } @@ -529,8 +568,9 @@ public SwerveController getSwerveController() { /** * Get the {@link SwerveDriveConfiguration} object. * - * @return The {@link SwerveDriveConfiguration} fpr the current drive. + * @return The {@link SwerveDriveConfiguration} for the current drive. */ + @AutoLogOutput(key = "SwerveDrive/Config") public SwerveDriveConfiguration getSwerveDriveConfiguration() { return swerveDrive.swerveDriveConfiguration; } @@ -545,6 +585,7 @@ public void lock() { * * @return The heading as a {@link Rotation2d} angle */ + @AutoLogOutput(key = "Odometry/Pitch") public Rotation2d getPitch() { return swerveDrive.getPitch(); } @@ -555,36 +596,34 @@ public void addFakeVisionReading() { new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp()); } - /*****************************************************************/ - /** 2024 SEASON-SPECIFIC FUNCTIONS, INCLUDED AS EXAMPLES */ - /** - * Get the distance to the speaker. - * - * @return Distance to speaker in meters. - */ - public double getDistanceToSpeaker() { - int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; - // Taken from PhotonUtils.getDistanceToPose - Pose3d speakerAprilTagPose = - AprilTagConstants.aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); - return getPose().getTranslation().getDistance(speakerAprilTagPose.toPose2d().getTranslation()); + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveDive/States/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = + new SwerveModuleState( + SwerveDriveTelemetry.measuredStates[(i * 2) + 1], + Rotation2d.fromDegrees(SwerveDriveTelemetry.measuredStates[i * 2])); + } + return states; + } + + /** Returns the desired states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveDive/States/Desired") + private SwerveModuleState[] getDesiredStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = + new SwerveModuleState( + SwerveDriveTelemetry.desiredStates[(i * 2) + 1], + Rotation2d.fromDegrees(SwerveDriveTelemetry.desiredStates[i * 2])); + } + return states; } - /** - * Get the yaw to aim at the speaker. - * - * @return {@link Rotation2d} of which you need to achieve. - */ - public Rotation2d getSpeakerYaw() { - int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; - // Taken from PhotonUtils.getYawToPose() - Pose3d speakerAprilTagPose = - AprilTagConstants.aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); - Translation2d relativeTrl = - speakerAprilTagPose.toPose2d().relativeTo(getPose()).getTranslation(); - return new Rotation2d(relativeTrl.getX(), relativeTrl.getY()) - .plus(swerveDrive.getOdometryHeading()); - } + /************************************************************************* */ + /** 2024 SEASON-SPECIFIC FUNCTIONS, INCLUDED AS EXAMPLES */ /** * Aim the robot at the speaker. @@ -600,10 +639,17 @@ public Command aimAtSpeaker(double tolerance) { 0, 0, controller.headingCalculate( - getHeading().getRadians(), getSpeakerYaw().getRadians()), + getHeading().getRadians(), + vision.getSpeakerYaw(swerveDrive.getOdometryHeading()).getRadians()), getHeading())); }) - .until(() -> getSpeakerYaw().minus(getHeading()).getDegrees() < tolerance); + .until( + () -> + vision + .getSpeakerYaw(swerveDrive.getOdometryHeading()) + .minus(getHeading()) + .getDegrees() + < tolerance); } /** diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 2ac10d55..2b922e07 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -27,10 +27,13 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import frc.robot.Constants.AprilTagConstants; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; @@ -485,4 +488,37 @@ public void addToVisionSim(VisionSystemSim systemSim) { } } } + + /*****************************************************************/ + /** 2024 SEASON-SPECIFIC FUNCTIONS, INCLUDED AS EXAMPLES */ + /** + * Get the distance to the speaker. + * + * @return Distance to speaker in meters. + */ + public double getDistanceToSpeaker() { + int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; + // Taken from PhotonUtils.getDistanceToPose + Pose3d speakerAprilTagPose = + AprilTagConstants.aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); + return currentPose + .get() + .getTranslation() + .getDistance(speakerAprilTagPose.toPose2d().getTranslation()); + } + + /** + * Get the yaw to aim at the speaker. + * + * @return {@link Rotation2d} of which you need to achieve. + */ + public Rotation2d getSpeakerYaw(Rotation2d odometryHeading) { + int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; + // Taken from PhotonUtils.getYawToPose() + Pose3d speakerAprilTagPose = + AprilTagConstants.aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); + Translation2d relativeTrl = + speakerAprilTagPose.toPose2d().relativeTo(currentPose.get()).getTranslation(); + return new Rotation2d(relativeTrl.getX(), relativeTrl.getY()).plus(odometryHeading); + } } From 8cd4e4cc4d03609c48eb9caff1032261102e7a9f Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Mon, 21 Oct 2024 16:17:44 -0700 Subject: [PATCH 28/57] Updating against YAGSL 2024.6.1.0 Mostly adding more pieces from the example project. modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java modified: src/main/java/frc/robot/util/CanDeviceId.java modified: vendordeps/yagsl.json --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 14 +- src/main/java/frc/robot/RobotContainer.java | 37 +++- .../swervedrive/SwerveSubsystem.java | 180 ++++++++++++------ src/main/java/frc/robot/util/CanDeviceId.java | 14 +- vendordeps/yagsl.json | 4 +- 6 files changed, 170 insertions(+), 81 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1afa3257..e6e88765 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -248,7 +248,7 @@ private AprilTagLayoutType(String name) { layoutString = new ObjectMapper().writeValueAsString(layout); } catch (JsonProcessingException e) { throw new RuntimeException( - "Failed to serialize AprilTag layout JSON " + toString() + "for Northstar"); + "Failed to serialize AprilTag layout JSON " + toString() + "for PhotonVision"); } } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cdcb893c..43fd44e5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,15 +13,12 @@ package frc.robot; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Constants.PowerConstants; import frc.robot.RobotContainer.Ports; import frc.robot.util.VirtualSubsystem; -import java.io.File; -import java.io.IOException; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LogTable; import org.littletonrobotics.junction.LoggedRobot; @@ -30,7 +27,6 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; -import swervelib.parser.SwerveParser; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -169,6 +165,8 @@ public void teleopInit() { // this line or comment it out. if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); + } else { + CommandScheduler.getInstance().cancelAll(); } m_robotContainer.setDriveMode(); m_robotContainer.setMotorBrake(true); @@ -183,13 +181,7 @@ public void teleopPeriodic() {} public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); - - // Try setting up swerve - try { - new SwerveParser(new File(Filesystem.getDeployDirectory(), "swerve")); - } catch (IOException e) { - throw new RuntimeException(e); - } + m_robotContainer.setDriveMode(); } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e9325125..6cada1fb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -189,6 +189,40 @@ private void configureBindings() { // Manually Re-Zero the Gyro driverXbox.y().onTrue(Commands.runOnce(() -> m_drivebase.zeroGyroWithAlliance())); + + // Example button bindings from YAGSL + // if (DriverStation.isTest()) + // { + // driverXbox.b().whileTrue(drivebase.sysIdDriveMotorCommand()); + // driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); + // driverXbox.y().whileTrue(drivebase.driveToDistanceCommand(1.0, 0.2)); + // driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro))); + // driverXbox.back().whileTrue(drivebase.centerModulesCommand()); + // driverXbox.leftBumper().onTrue(Commands.none()); + // driverXbox.rightBumper().onTrue(Commands.none()); + // drivebase.setDefaultCommand( + // !RobotBase.isSimulation() ? driveFieldOrientedAnglularVelocity : + // driveFieldOrientedDirectAngleSim); + // } else + // { + // driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro))); + // driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading)); + // driverXbox.b().whileTrue( + // Commands.deferredProxy(() -> drivebase.driveToPose( + // new Pose2d(new Translation2d(4, 4), + // Rotation2d.fromDegrees(0))) + // )); + // driverXbox.y().whileTrue(drivebase.aimAtSpeaker(2)); + // driverXbox.start().whileTrue(Commands.none()); + // driverXbox.back().whileTrue(Commands.none()); + // driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, + // drivebase).repeatedly()); + // driverXbox.rightBumper().onTrue(Commands.none()); + // drivebase.setDefaultCommand( + // !RobotBase.isSimulation() ? driveFieldOrientedDirectAngle : + // driveFieldOrientedDirectAngleSim); + // } + } /** @@ -205,8 +239,7 @@ public Command getAutonomousCommand() { } public void setDriveMode() { - - // drivebase.setDefaultCommand(); + configureBindings(); } public void setMotorBrake(boolean brake) { diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 1a9331bd..adcb60d2 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -27,6 +27,7 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -37,11 +38,13 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.Constants.AutonConstants; import frc.robot.subsystems.vision.Vision; import java.io.File; +import java.util.Arrays; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -113,6 +116,16 @@ public SwerveSubsystem(File directory) { swerveDrive.setCosineCompensator(true); } + // Correct for skew that gets worse as angular velocity increases. Start with a coefficient of + // 0.1 + swerveDrive.setAngularVelocityCompensation(true, true, 0.1); + // Enable if you want to resynchronize your absolute encoders and motor encoders periodically + // when they are not moving. + swerveDrive.setModuleEncoderAutoSynchronize(false, 1); + // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. + // Throws warning if not possible + swerveDrive.pushOffsetsToEncoders(); + if (visionDriveTest) { setupPhotonVision(); // Stop the odometry thread if we are using vision that way we can synchronize updates better. @@ -389,6 +402,32 @@ public Command sysIdAngleMotorCommand() { kDynamicTimeout); } + /** + * Returns a Command that centers the modules of the SwerveDrive subsystem. + * + * @return a Command that centers the modules of the SwerveDrive subsystem + */ + public Command centerModulesCommand() { + return run(() -> Arrays.asList(swerveDrive.getModules()).forEach(it -> it.setAngle(0.0))); + } + + /** + * Returns a Command that drives the swerve drive to a specific distance at a given speed. + * + * @param distanceInMeters the distance to drive in meters + * @param speedInMetersPerSecond the speed at which to drive in meters per second + * @return a Command that drives the swerve drive to a specific distance at a given speed + */ + public Command driveToDistanceCommand(double distanceInMeters, double speedInMetersPerSecond) { + return Commands.deferredProxy( + () -> + Commands.run(() -> drive(new ChassisSpeeds(speedInMetersPerSecond, 0, 0)), this) + .until( + () -> + swerveDrive.getPose().getTranslation().getDistance(new Translation2d(0, 0)) + > distanceInMeters)); + } + /************************************************************************* */ /* UTILITY SECTION -- Utility methods */ @@ -402,17 +441,6 @@ public SwerveDriveKinematics getKinematics() { return swerveDrive.kinematics; } - /** - * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when - * calling this method. However, if either gyro angle or module position is reset, this must be - * called in order for odometry to keep working. - * - * @param initialHolonomicPose The pose to set the odometry to - */ - public void resetOdometry(Pose2d initialHolonomicPose) { - swerveDrive.resetOdometry(initialHolonomicPose); - } - /** * Gets the current pose (position and rotation) of the robot, as reported by odometry. * @@ -423,6 +451,48 @@ public Pose2d getPose() { return swerveDrive.getPose(); } + /** + * Gets the current yaw angle of the robot, as reported by the swerve pose estimator in the + * underlying drivebase. Note, this is not the raw gyro reading, this may be corrected from calls + * to resetOdometry(). + * + * @return The yaw angle + */ + @AutoLogOutput(key = "Odometry/Heading") + public Rotation2d getHeading() { + return getPose().getRotation(); + } + + /** + * Gets the current field-relative velocity (x, y and omega) of the robot + * + * @return A ChassisSpeeds object of the current field-relative velocity + */ + @AutoLogOutput(key = "Odometry/FieldVelocity") + public ChassisSpeeds getFieldVelocity() { + return swerveDrive.getFieldVelocity(); + } + + /** + * Gets the current velocity (x, y and omega) of the robot + * + * @return A {@link ChassisSpeeds} object of the current velocity + */ + @AutoLogOutput(key = "Odometry/RobotVelocity") + public ChassisSpeeds getRobotVelocity() { + return swerveDrive.getRobotVelocity(); + } + + /** + * Gets the current pitch angle of the robot, as reported by the imu. + * + * @return The heading as a {@link Rotation2d} angle + */ + @AutoLogOutput(key = "Odometry/Pitch") + public Rotation2d getPitch() { + return swerveDrive.getPitch(); + } + /** * Set chassis speeds with closed-loop velocity control. * @@ -432,6 +502,17 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { swerveDrive.setChassisSpeeds(chassisSpeeds); } + /** + * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when + * calling this method. However, if either gyro angle or module position is reset, this must be + * called in order for odometry to keep working. + * + * @param initialHolonomicPose The pose to set the odometry to + */ + public void resetOdometry(Pose2d initialHolonomicPose) { + swerveDrive.resetOdometry(initialHolonomicPose); + } + /** * Post the trajectory to the field. * @@ -482,18 +563,6 @@ public void setMotorBrake(boolean brake) { swerveDrive.setMotorIdleMode(brake); } - /** - * Gets the current yaw angle of the robot, as reported by the swerve pose estimator in the - * underlying drivebase. Note, this is not the raw gyro reading, this may be corrected from calls - * to resetOdometry(). - * - * @return The yaw angle - */ - @AutoLogOutput(key = "Odometry/Heading") - public Rotation2d getHeading() { - return getPose().getRotation(); - } - /** * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which * direction. The other for the angle of the robot. @@ -513,7 +582,7 @@ public ChassisSpeeds getTargetSpeeds( headingX, headingY, getHeading().getRadians(), - MAX_LINEAR_SPEED); + swerveDrive.getMaximumVelocity()); } /** @@ -533,27 +602,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d an scaledInputs.getY(), angle.getRadians(), getHeading().getRadians(), - MAX_LINEAR_SPEED); - } - - /** - * Gets the current field-relative velocity (x, y and omega) of the robot - * - * @return A ChassisSpeeds object of the current field-relative velocity - */ - @AutoLogOutput(key = "Odometry/FieldVelocity") - public ChassisSpeeds getFieldVelocity() { - return swerveDrive.getFieldVelocity(); - } - - /** - * Gets the current velocity (x, y and omega) of the robot - * - * @return A {@link ChassisSpeeds} object of the current velocity - */ - @AutoLogOutput(key = "Odometry/RobotVelocity") - public ChassisSpeeds getRobotVelocity() { - return swerveDrive.getRobotVelocity(); + swerveDrive.getMaximumVelocity()); } /** @@ -580,20 +629,34 @@ public void lock() { swerveDrive.lockPose(); } + /** Add a fake vision reading for testing purposes. */ + public void addFakeVisionReading() { + swerveDrive.addVisionMeasurement( + new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp()); + } + /** - * Gets the current pitch angle of the robot, as reported by the imu. + * Sets the maximum speed of the swerve drive. * - * @return The heading as a {@link Rotation2d} angle + * @param maximumSpeedInMetersPerSecond the maximum speed to set for the swerve drive in meters + * per second */ - @AutoLogOutput(key = "Odometry/Pitch") - public Rotation2d getPitch() { - return swerveDrive.getPitch(); + public void setMaximumSpeed(double maximumSpeedInMetersPerSecond) { + swerveDrive.setMaximumSpeed( + maximumSpeedInMetersPerSecond, + false, + swerveDrive.swerveDriveConfiguration.physicalCharacteristics.optimalVoltage); } - /** Add a fake vision reading for testing purposes. */ - public void addFakeVisionReading() { - swerveDrive.addVisionMeasurement( - new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp()); + /** + * Replaces the swerve module feedforward with a new SimpleMotorFeedforward object. + * + * @param kS the static gain of the feedforward + * @param kV the velocity gain of the feedforward + * @param kA the acceleration gain of the feedforward + */ + public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) { + swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA)); } /** Returns the module states (turn angles and drive velocities) for all of the modules. */ @@ -645,10 +708,11 @@ public Command aimAtSpeaker(double tolerance) { }) .until( () -> - vision - .getSpeakerYaw(swerveDrive.getOdometryHeading()) - .minus(getHeading()) - .getDegrees() + Math.abs( + vision + .getSpeakerYaw(swerveDrive.getOdometryHeading()) + .minus(getHeading()) + .getDegrees()) < tolerance); } diff --git a/src/main/java/frc/robot/util/CanDeviceId.java b/src/main/java/frc/robot/util/CanDeviceId.java index 6421843d..00b657cf 100644 --- a/src/main/java/frc/robot/util/CanDeviceId.java +++ b/src/main/java/frc/robot/util/CanDeviceId.java @@ -17,12 +17,12 @@ /** Class for wrapping CAN devices with a name and functionality */ public class CanDeviceId { - private final int mDeviceNumber; - private final String mBus; + private final int m_DeviceNumber; + private final String m_Bus; public CanDeviceId(int deviceNumber, String bus) { - mDeviceNumber = deviceNumber; - mBus = bus; + m_DeviceNumber = deviceNumber; + m_Bus = bus; } /** Use the default bus name (empty string) */ @@ -32,16 +32,16 @@ public CanDeviceId(int deviceNumber) { /** Get the CAN ID value for a named device */ public int getDeviceNumber() { - return mDeviceNumber; + return m_DeviceNumber; } /** Get the CAN bus name for a named device */ public String getBus() { - return mBus; + return m_Bus; } /** Check whether two named devices are, in fact, the same */ public boolean equals(CanDeviceId other) { - return other.mDeviceNumber == mDeviceNumber && other.mBus == mBus; + return other.m_DeviceNumber == m_DeviceNumber && other.m_Bus == m_Bus; } } diff --git a/vendordeps/yagsl.json b/vendordeps/yagsl.json index 9a4905ee..d4cb6fed 100644 --- a/vendordeps/yagsl.json +++ b/vendordeps/yagsl.json @@ -1,7 +1,7 @@ { "fileName": "yagsl.json", "name": "YAGSL", - "version": "2024.5.0.4", + "version": "2024.6.1.0", "frcYear": "2024", "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "swervelib", "artifactId": "YAGSL-java", - "version": "2024.5.0.4" + "version": "2024.6.1.0" } ], "jniDependencies": [], From 38047b23f6740398c7ff79e575275c0a27ffc34f Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Mon, 21 Oct 2024 17:21:04 -0700 Subject: [PATCH 29/57] Add modified versions of YAGSL modules for CTRE Pro In order to use modified versions of the CTRE connection modules from YAGSL, needed to modify several files to deal with the parsing chain. Not sure if this is the best long-term solution, but I think it will work for now. The next step will be to modify the ``YAGSL_AzRBSI/*_RBSI.java`` hardware control files (CANCoder, Pigeon2, TalonFX) to allow access to the Phoenix Pro features we want access to. modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java new file: src/main/java/frc/robot/util/YAGSL_AzRBSI/CANCoderSwerve_RBSI.java new file: src/main/java/frc/robot/util/YAGSL_AzRBSI/DeviceJson_RBSI.java new file: src/main/java/frc/robot/util/YAGSL_AzRBSI/Pigeon2Swerve_RBSI.java new file: src/main/java/frc/robot/util/YAGSL_AzRBSI/README new file: src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveDriveJson_RBSI.java new file: src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveParser_RBSI.java new file: src/main/java/frc/robot/util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../swervedrive/SwerveSubsystem.java | 6 +- .../YAGSL_AzRBSI/CANCoderSwerve_RBSI.java | 210 +++++++++ .../util/YAGSL_AzRBSI/DeviceJson_RBSI.java | 201 ++++++++ .../util/YAGSL_AzRBSI/Pigeon2Swerve_RBSI.java | 161 +++++++ .../java/frc/robot/util/YAGSL_AzRBSI/README | 19 + .../YAGSL_AzRBSI/SwerveDriveJson_RBSI.java | 32 ++ .../util/YAGSL_AzRBSI/SwerveParser_RBSI.java | 229 +++++++++ .../util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java | 446 ++++++++++++++++++ 9 files changed, 1302 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/util/YAGSL_AzRBSI/CANCoderSwerve_RBSI.java create mode 100644 src/main/java/frc/robot/util/YAGSL_AzRBSI/DeviceJson_RBSI.java create mode 100644 src/main/java/frc/robot/util/YAGSL_AzRBSI/Pigeon2Swerve_RBSI.java create mode 100644 src/main/java/frc/robot/util/YAGSL_AzRBSI/README create mode 100644 src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveDriveJson_RBSI.java create mode 100644 src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveParser_RBSI.java create mode 100644 src/main/java/frc/robot/util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6cada1fb..f5e8e2ff 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,7 +36,6 @@ import frc.robot.subsystems.flywheel_example.Flywheel; import frc.robot.subsystems.flywheel_example.FlywheelIO; import frc.robot.subsystems.flywheel_example.FlywheelIOSim; -import frc.robot.subsystems.flywheel_example.FlywheelIOTalonFX; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; @@ -80,7 +79,7 @@ public RobotContainer() { // Real robot, instantiate hardware IO implementations // YAGSL drivebase, get config from deploy directory m_drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); - m_flywheel = new Flywheel(new FlywheelIOTalonFX()); + m_flywheel = new Flywheel(new FlywheelIOSim()); // new Flywheel(new FlywheelIOTalonFX()); m_vision = new Vision( this::getAprilTagLayoutType, diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index adcb60d2..d2184de7 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -43,6 +43,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.Constants.AutonConstants; import frc.robot.subsystems.vision.Vision; +import frc.robot.util.YAGSL_AzRBSI.SwerveParser_RBSI; import java.io.File; import java.util.Arrays; import java.util.function.DoubleSupplier; @@ -56,7 +57,6 @@ import swervelib.math.SwerveMath; import swervelib.parser.SwerveControllerConfiguration; import swervelib.parser.SwerveDriveConfiguration; -import swervelib.parser.SwerveParser; import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @@ -99,9 +99,9 @@ public SwerveSubsystem(File directory) { // created. SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; try { - swerveDrive = new SwerveParser(directory).createSwerveDrive(MAX_LINEAR_SPEED); + swerveDrive = new SwerveParser_RBSI(directory).createSwerveDrive(MAX_LINEAR_SPEED); // Alternative method if you don't want to supply the conversion factor via JSON files. - // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, + // swerveDrive = new SwerveParser_RBSI(directory).createSwerveDrive(maximumSpeed, // angleConversionFactor, driveConversionFactor); } catch (Exception e) { throw new RuntimeException(e); diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/CANCoderSwerve_RBSI.java b/src/main/java/frc/robot/util/YAGSL_AzRBSI/CANCoderSwerve_RBSI.java new file mode 100644 index 00000000..e45393b6 --- /dev/null +++ b/src/main/java/frc/robot/util/YAGSL_AzRBSI/CANCoderSwerve_RBSI.java @@ -0,0 +1,210 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright (c) 2024 FRC 3481 +// https://github.com/BroncBotz3481/YAGSL +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util.YAGSL_AzRBSI; + +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CANcoderConfigurator; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; +import com.ctre.phoenix6.signals.MagnetHealthValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.telemetry.Alert; + +/** Swerve Absolute Encoder for CTRE CANCoders. */ +public class CANCoderSwerve_RBSI extends SwerveAbsoluteEncoder { + + /** Wait time for status frames to show up. */ + public static double STATUS_TIMEOUT_SECONDS = 0.02; + + /** CANCoder with WPILib sendable and support. */ + public CANcoder encoder; + + /** An {@link Alert} for if the CANCoder magnet field is less than ideal. */ + private Alert magnetFieldLessThanIdeal; + + /** An {@link Alert} for if the CANCoder reading is faulty. */ + private Alert readingFaulty; + + /** An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored. */ + private Alert readingIgnored; + + /** An {@link Alert} for if the absolute encoder offset cannot be set. */ + private Alert cannotSetOffset; + + /** + * Initialize the CANCoder on the standard CANBus. + * + * @param id CAN ID. + */ + public CANCoderSwerve_RBSI(int id) { + // Empty string uses the default canbus for the system + this(id, ""); + } + + /** + * Initialize the CANCoder on the CANivore. + * + * @param id CAN ID. + * @param canbus CAN bus to initialize it on. + */ + public CANCoderSwerve_RBSI(int id, String canbus) { + encoder = new CANcoder(id, canbus); + magnetFieldLessThanIdeal = + new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.", + Alert.AlertType.WARNING); + readingFaulty = + new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " reading was faulty.", + Alert.AlertType.WARNING); + readingIgnored = + new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.", + Alert.AlertType.WARNING); + cannotSetOffset = + new Alert( + "Encoders", + "Failure to set CANCoder " + encoder.getDeviceID() + " Absolute Encoder Offset", + Alert.AlertType.WARNING); + } + + /** Reset the encoder to factory defaults. */ + @Override + public void factoryDefault() { + encoder.getConfigurator().apply(new CANcoderConfiguration()); + } + + /** Clear sticky faults on the encoder. */ + @Override + public void clearStickyFaults() { + encoder.clearStickyFaults(); + } + + /** + * Configure the absolute encoder to read from [0, 360) per second. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) { + CANcoderConfigurator cfg = encoder.getConfigurator(); + MagnetSensorConfigs magnetSensorConfiguration = new MagnetSensorConfigs(); + cfg.refresh(magnetSensorConfiguration); + cfg.apply( + magnetSensorConfiguration + .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1) + .withSensorDirection( + inverted + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive)); + } + + /** + * Get the absolute position of the encoder. Sets {@link SwerveAbsoluteEncoder#readingError} on + * erroneous readings. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() { + readingError = false; + MagnetHealthValue strength = encoder.getMagnetHealth().getValue(); + + magnetFieldLessThanIdeal.set(strength != MagnetHealthValue.Magnet_Green); + if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red) { + readingError = true; + readingFaulty.set(true); + return 0; + } else { + readingFaulty.set(false); + } + + StatusSignal angle = encoder.getAbsolutePosition(); + + // Taken from democat's library. + // Source: + // https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74 + for (int i = 0; i < maximumRetries; i++) { + if (angle.getStatus() == StatusCode.OK) { + break; + } + angle = angle.waitForUpdate(STATUS_TIMEOUT_SECONDS); + } + if (angle.getStatus() != StatusCode.OK) { + readingError = true; + readingIgnored.set(true); + } else { + readingIgnored.set(false); + } + + return angle.getValue() * 360; + } + + /** + * Get the instantiated absolute encoder Object. + * + * @return Absolute encoder object. + */ + @Override + public Object getAbsoluteEncoder() { + return encoder; + } + + /** + * Sets the Absolute Encoder Offset within the CANcoder's Memory. + * + * @param offset the offset the Absolute Encoder uses as the zero point in degrees. + * @return if setting Absolute Encoder Offset was successful or not. + */ + @Override + public boolean setAbsoluteEncoderOffset(double offset) { + CANcoderConfigurator cfg = encoder.getConfigurator(); + MagnetSensorConfigs magCfg = new MagnetSensorConfigs(); + StatusCode error = cfg.refresh(magCfg); + if (error != StatusCode.OK) { + return false; + } + error = cfg.apply(magCfg.withMagnetOffset(offset / 360)); + cannotSetOffset.setText( + "Failure to set CANCoder " + + encoder.getDeviceID() + + " Absolute Encoder Offset Error: " + + error); + if (error == StatusCode.OK) { + cannotSetOffset.set(false); + return true; + } + cannotSetOffset.set(true); + return false; + } + + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() { + return encoder.getVelocity().getValue() * 360; + } +} diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/DeviceJson_RBSI.java b/src/main/java/frc/robot/util/YAGSL_AzRBSI/DeviceJson_RBSI.java new file mode 100644 index 00000000..6df0c2e4 --- /dev/null +++ b/src/main/java/frc/robot/util/YAGSL_AzRBSI/DeviceJson_RBSI.java @@ -0,0 +1,201 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright (c) 2024 FRC 3481 +// https://github.com/BroncBotz3481/YAGSL +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util.YAGSL_AzRBSI; + +import static swervelib.telemetry.SwerveDriveTelemetry.canIdWarning; +import static swervelib.telemetry.SwerveDriveTelemetry.i2cLockupWarning; +import static swervelib.telemetry.SwerveDriveTelemetry.serialCommsIssueWarning; + +import com.revrobotics.SparkRelativeEncoder.Type; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.I2C; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.SerialPort.Port; +import swervelib.encoders.AnalogAbsoluteEncoderSwerve; +import swervelib.encoders.CanAndMagSwerve; +import swervelib.encoders.PWMDutyCycleEncoderSwerve; +import swervelib.encoders.SparkMaxAnalogEncoderSwerve; +import swervelib.encoders.SparkMaxEncoderSwerve; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.imu.ADIS16448Swerve; +import swervelib.imu.ADIS16470Swerve; +import swervelib.imu.ADXRS450Swerve; +import swervelib.imu.AnalogGyroSwerve; +import swervelib.imu.CanandgyroSwerve; +import swervelib.imu.NavXSwerve; +import swervelib.imu.PigeonSwerve; +import swervelib.imu.SwerveIMU; +import swervelib.motors.SparkFlexSwerve; +import swervelib.motors.SparkMaxBrushedMotorSwerve; +import swervelib.motors.SparkMaxSwerve; +import swervelib.motors.SwerveMotor; +import swervelib.motors.TalonSRXSwerve; + +/** Device JSON parsed class. Used to access the JSON data. */ +public class DeviceJson_RBSI { + + /** The device type, e.g. pigeon/pigeon2/sparkmax/talonfx/navx */ + public String type; + + /** The CAN ID or pin ID of the device. */ + public int id; + + /** The CAN bus name which the device resides on if using CAN. */ + public String canbus = ""; + + /** + * Create a {@link SwerveAbsoluteEncoder} from the current configuration. + * + * @param motor {@link SwerveMotor} of which attached encoders will be created from, only used + * when the type is "attached" or "canandencoder". + * @return {@link SwerveAbsoluteEncoder} given. + */ + public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor) { + if (id > 40) { + canIdWarning.set(true); + } + switch (type) { + case "none": + return null; + case "integrated": + case "attached": + case "canandmag": + case "canandcoder": + return new SparkMaxEncoderSwerve(motor, 360); + case "sparkmax_analog": + return new SparkMaxAnalogEncoderSwerve(motor, 3.3); + case "sparkmax_analog5v": + return new SparkMaxAnalogEncoderSwerve(motor, 5); + case "canandcoder_can": + case "canandmag_can": + return new CanAndMagSwerve(id); + case "ctre_mag": + case "rev_hex": + case "throughbore": + case "am_mag": + case "dutycycle": + return new PWMDutyCycleEncoderSwerve(id); + case "thrifty": + case "ma3": + case "analog": + return new AnalogAbsoluteEncoderSwerve(id); + case "cancoder": + return new CANCoderSwerve_RBSI(id, canbus != null ? canbus : ""); + default: + throw new RuntimeException(type + " is not a recognized absolute encoder type."); + } + } + + /** + * Create a {@link SwerveIMU} from the given configuration. + * + * @return {@link SwerveIMU} given. + */ + public SwerveIMU createIMU() { + if (id > 40) { + canIdWarning.set(true); + } + switch (type) { + case "adis16448": + return new ADIS16448Swerve(); + case "adis16470": + return new ADIS16470Swerve(); + case "adxrs450": + return new ADXRS450Swerve(); + case "analog": + return new AnalogGyroSwerve(id); + case "canandgyro": + return new CanandgyroSwerve(id); + case "navx": + case "navx_spi": + return new NavXSwerve(SPI.Port.kMXP); + case "navx_i2c": + DriverStation.reportWarning( + "WARNING: There exists an I2C lockup issue on the roboRIO that could occur, more information here: " + + "\nhttps://docs.wpilib.org/en/stable/docs/yearly-overview/known-issues" + + ".html#onboard-i2c-causing-system-lockups", + false); + i2cLockupWarning.set(true); + return new NavXSwerve(I2C.Port.kMXP); + case "navx_usb": + DriverStation.reportWarning( + "WARNING: There is issues when using USB camera's and the NavX like this!\n" + + "https://pdocs.kauailabs.com/navx-mxp/guidance/selecting-an-interface/", + false); + serialCommsIssueWarning.set(true); + return new NavXSwerve(Port.kUSB); + case "navx_mxp_serial": + serialCommsIssueWarning.set(true); + return new NavXSwerve(Port.kMXP); + case "pigeon": + return new PigeonSwerve(id); + case "pigeon2": + return new Pigeon2Swerve_RBSI(id, canbus != null ? canbus : ""); + default: + throw new RuntimeException(type + " is not a recognized imu/gyroscope type."); + } + } + + /** + * Create a {@link SwerveMotor} from the given configuration. + * + * @param isDriveMotor If the motor being generated is a drive motor. + * @return {@link SwerveMotor} given. + */ + public SwerveMotor createMotor(boolean isDriveMotor) { + if (id > 40) { + canIdWarning.set(true); + } + switch (type) { + case "sparkmax_brushed": + switch (canbus) { + case "greyhill_63r256": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, false); + case "srx_mag_encoder": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, false); + case "throughbore": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 8192, false); + case "throughbore_dataport": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 8192, true); + case "greyhill_63r256_dataport": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, true); + case "srx_mag_encoder_dataport": + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, true); + default: + if (isDriveMotor) { + throw new RuntimeException( + "Spark MAX " + id + " MUST have a encoder attached to the motor controller."); + } + // We are creating a motor for an angle motor which will use the absolute encoder + // attached to the data port. + return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 0, false); + } + case "neo": + case "sparkmax": + return new SparkMaxSwerve(id, isDriveMotor); + case "sparkflex": + return new SparkFlexSwerve(id, isDriveMotor); + case "falcon": + case "kraken": + case "talonfx": + return new TalonFXSwerve_RBSI(id, canbus != null ? canbus : "", isDriveMotor); + case "talonsrx": + return new TalonSRXSwerve(id, isDriveMotor); + default: + throw new RuntimeException(type + " is not a recognized motor type."); + } + } +} diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/Pigeon2Swerve_RBSI.java b/src/main/java/frc/robot/util/YAGSL_AzRBSI/Pigeon2Swerve_RBSI.java new file mode 100644 index 00000000..0b54ff60 --- /dev/null +++ b/src/main/java/frc/robot/util/YAGSL_AzRBSI/Pigeon2Swerve_RBSI.java @@ -0,0 +1,161 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright (c) 2024 FRC 3481 +// https://github.com/BroncBotz3481/YAGSL +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util.YAGSL_AzRBSI; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.configs.Pigeon2Configurator; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.Optional; +import swervelib.imu.SwerveIMU; + +/** SwerveIMU interface for the Pigeon2 */ +public class Pigeon2Swerve_RBSI extends SwerveIMU { + + /** Wait time for status frames to show up. */ + public static double STATUS_TIMEOUT_SECONDS = 0.04; + + /** Pigeon2 IMU device. */ + Pigeon2 imu; + + /** Offset for the Pigeon 2. */ + private Rotation3d offset = new Rotation3d(); + + /** Inversion for the gyro */ + private boolean invertedIMU = false; + + /** Pigeon2 configurator. */ + private Pigeon2Configurator cfg; + + /** + * Generate the SwerveIMU for pigeon. + * + * @param canid CAN ID for the pigeon + * @param canbus CAN Bus name the pigeon resides on. + */ + public Pigeon2Swerve_RBSI(int canid, String canbus) { + imu = new Pigeon2(canid, canbus); + this.cfg = imu.getConfigurator(); + SmartDashboard.putData(imu); + } + + /** + * Generate the SwerveIMU for pigeon. + * + * @param canid CAN ID for the pigeon + */ + public Pigeon2Swerve_RBSI(int canid) { + this(canid, ""); + } + + /** Reset IMU to factory default. */ + @Override + public void factoryDefault() { + Pigeon2Configuration config = new Pigeon2Configuration(); + + // Compass utilization causes readings to jump dramatically in some cases. + cfg.apply(config.Pigeon2Features.withEnableCompass(false)); + } + + /** Clear sticky faults on IMU. */ + @Override + public void clearStickyFaults() { + imu.clearStickyFaults(); + } + + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) { + this.offset = offset; + } + + /** + * Set the gyro to invert its default direction + * + * @param invertIMU invert gyro direction + */ + public void setInverted(boolean invertIMU) { + invertedIMU = invertIMU; + } + + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRawRotation3d() { + Rotation3d reading = imu.getRotation3d(); + return invertedIMU ? reading.unaryMinus() : reading; + } + + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() { + return getRawRotation3d().minus(offset); + } + + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration + * isn't supported returns empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() { + // TODO: Switch to suppliers. + StatusSignal xAcc = imu.getAccelerationX(); + StatusSignal yAcc = imu.getAccelerationY(); + StatusSignal zAcc = imu.getAccelerationZ(); + + return Optional.of( + new Translation3d( + xAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(), + yAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(), + zAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue()) + .times(9.81 / 16384.0)); + } + + /** + * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported + * returns empty. + * + * @return {@link Double} of the rotation rate as an {@link Optional}. + */ + public double getRate() { + return imu.getRate(); + } + + /** + * Get the instantiated IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() { + return imu; + } +} diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/README b/src/main/java/frc/robot/util/YAGSL_AzRBSI/README new file mode 100644 index 00000000..18411457 --- /dev/null +++ b/src/main/java/frc/robot/util/YAGSL_AzRBSI/README @@ -0,0 +1,19 @@ +This directory contains modified versions of various routines from the YAGSL +library (https://github.com/BroncBotz3481/YAGSL). The purpose of the modified +routines is to allow access to the CRTE Phoenix Pro features on supported +devices from within YAGSL -- which otherwise ignores Phoenix Pro. + +Since the Az-RBSI is designed around a CTRE/SDS swerve base, yet we wish to +make use of the features of the YAGSL, it is necessary to extend these +particular routines to allow for Phoenix Pro features (if enabled / licensed). + +NOTE for developers: it will be necessary, from time to time, to merge in any +modifications to these files from the main YAGSL repo to maintain compliant +functionality with the remainder of YAGSL. + +::: + +Last merged from YAGSL 21-Oct-2024, v2024.6.1.0 + +----------------- +21-Oct-2024, TPEB diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveDriveJson_RBSI.java b/src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveDriveJson_RBSI.java new file mode 100644 index 00000000..f06ab4f3 --- /dev/null +++ b/src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveDriveJson_RBSI.java @@ -0,0 +1,32 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright (c) 2024 FRC 3481 +// https://github.com/BroncBotz3481/YAGSL +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util.YAGSL_AzRBSI; + +/** + * {@link swervelib.SwerveDrive} JSON parsed class. Used to access parsed data from the + * swervedrive.json file. + */ +public class SwerveDriveJson_RBSI { + + /** Robot IMU used to determine heading of the robot. */ + public DeviceJson_RBSI imu; + + /** Invert the IMU of the robot. */ + public boolean invertedIMU; + + /** Module JSONs in order clockwise order starting from front left. */ + public String[] modules; +} diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveParser_RBSI.java b/src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveParser_RBSI.java new file mode 100644 index 00000000..d899f876 --- /dev/null +++ b/src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveParser_RBSI.java @@ -0,0 +1,229 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright (c) 2024 FRC 3481 +// https://github.com/BroncBotz3481/YAGSL +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util.YAGSL_AzRBSI; + +import com.fasterxml.jackson.databind.JsonNode; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import java.io.File; +import java.io.IOException; +import java.util.HashMap; +import swervelib.SwerveDrive; +import swervelib.SwerveModule; +import swervelib.math.SwerveMath; +import swervelib.parser.SwerveDriveConfiguration; +import swervelib.parser.SwerveModuleConfiguration; +import swervelib.parser.json.ControllerPropertiesJson; +import swervelib.parser.json.ModuleJson; +import swervelib.parser.json.PIDFPropertiesJson; +import swervelib.parser.json.PhysicalPropertiesJson; + +/** Helper class used to parse the JSON directory with specified configuration options. */ +public class SwerveParser_RBSI { + + /** Module number mapped to the JSON name. */ + private static final HashMap moduleConfigs = new HashMap<>(); + + /** Parsed swervedrive.json */ + public static SwerveDriveJson_RBSI swerveDriveJson; + + /** Parsed controllerproperties.json */ + public static ControllerPropertiesJson controllerPropertiesJson; + + /** Parsed modules/pidfproperties.json */ + public static PIDFPropertiesJson pidfPropertiesJson; + + /** Parsed modules/physicalproperties.json */ + public static PhysicalPropertiesJson physicalPropertiesJson; + + /** Array holding the module jsons given in {@link SwerveDriveJson_RBSI}. */ + public static ModuleJson[] moduleJsons; + + /** + * Construct a swerve parser. Will throw an error if there is a missing file. + * + * @param directory Directory with swerve configurations. + * @throws IOException if a file doesn't exist. + */ + public SwerveParser_RBSI(File directory) throws IOException { + checkDirectory(directory); + swerveDriveJson = + new ObjectMapper() + .readValue(new File(directory, "swervedrive.json"), SwerveDriveJson_RBSI.class); + controllerPropertiesJson = + new ObjectMapper() + .readValue( + new File(directory, "controllerproperties.json"), ControllerPropertiesJson.class); + pidfPropertiesJson = + new ObjectMapper() + .readValue( + new File(directory, "modules/pidfproperties.json"), PIDFPropertiesJson.class); + physicalPropertiesJson = + new ObjectMapper() + .readValue( + new File(directory, "modules/physicalproperties.json"), + PhysicalPropertiesJson.class); + moduleJsons = new ModuleJson[swerveDriveJson.modules.length]; + for (int i = 0; i < moduleJsons.length; i++) { + moduleConfigs.put(swerveDriveJson.modules[i], i); + File moduleFile = new File(directory, "modules/" + swerveDriveJson.modules[i]); + assert moduleFile.exists(); + moduleJsons[i] = new ObjectMapper().readValue(moduleFile, ModuleJson.class); + } + } + + /** + * Get the swerve module by the json name. + * + * @param name JSON name. + * @param driveConfiguration {@link SwerveDriveConfiguration} to pull from. + * @return {@link SwerveModuleConfiguration} based on the file. + */ + public static SwerveModule getModuleConfigurationByName( + String name, SwerveDriveConfiguration driveConfiguration) { + return driveConfiguration.modules[moduleConfigs.get(name + ".json")]; + } + + /** + * Open JSON file. + * + * @param file JSON File to open. + * @return JsonNode of file. + */ + private JsonNode openJson(File file) { + try { + return new ObjectMapper().readTree(file); + } catch (IOException e) { + throw new RuntimeException(e); + } + } + + /** + * Check directory structure. + * + * @param directory JSON Configuration Directory + */ + private void checkDirectory(File directory) { + assert new File(directory, "swervedrive.json").exists(); + assert new File(directory, "controllerproperties.json").exists(); + assert new File(directory, "modules").exists() && new File(directory, "modules").isDirectory(); + assert new File(directory, "modules/pidfproperties.json").exists(); + assert new File(directory, "modules/physicalproperties.json").exists(); + } + + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular + * acceleration used in {@link swervelib.SwerveController} and drive feedforward in {@link + * SwerveMath#createDriveFeedforward(double, double, double)}. + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive(double maxSpeed) { + return createSwerveDrive( + SwerveMath.createDriveFeedforward( + physicalPropertiesJson.optimalVoltage, + maxSpeed, + physicalPropertiesJson.wheelGripCoefficientOfFriction), + maxSpeed); + } + + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular + * acceleration used in {@link swervelib.SwerveController} and drive feedforward in {@link + * SwerveMath#createDriveFeedforward(double, double, double)}. + * @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor + * controller PID loop units to degrees, usually created using {@link + * SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. + * @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop + * units to meters per rotation, usually created using {@link + * SwerveMath#calculateMetersPerRotation(double, double, double)}. + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive( + double maxSpeed, double angleMotorConversionFactor, double driveMotorConversion) { + physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor; + physicalPropertiesJson.conversionFactor.drive = driveMotorConversion; + return createSwerveDrive( + SwerveMath.createDriveFeedforward( + physicalPropertiesJson.optimalVoltage, + maxSpeed, + physicalPropertiesJson.wheelGripCoefficientOfFriction), + maxSpeed); + } + + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param driveFeedforward Drive feedforward to use for swerve modules, should be created using + * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. + * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration + * in {@link swervelib.SwerveController} of the robot. + * @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor + * controller PID loop units to degrees, usually created using {@link + * SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. + * @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop + * units to meters per rotation, usually created using {@link + * SwerveMath#calculateMetersPerRotation(double, double, double)}. + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive( + SimpleMotorFeedforward driveFeedforward, + double maxSpeed, + double angleMotorConversionFactor, + double driveMotorConversion) { + physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor; + physicalPropertiesJson.conversionFactor.drive = driveMotorConversion; + return createSwerveDrive(driveFeedforward, maxSpeed); + } + + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param driveFeedforward Drive feedforward to use for swerve modules, should be created using + * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. + * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration + * in {@link swervelib.SwerveController} of the robot + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed) { + SwerveModuleConfiguration[] moduleConfigurations = + new SwerveModuleConfiguration[moduleJsons.length]; + for (int i = 0; i < moduleConfigurations.length; i++) { + ModuleJson module = moduleJsons[i]; + moduleConfigurations[i] = + module.createModuleConfiguration( + pidfPropertiesJson.angle, + pidfPropertiesJson.drive, + physicalPropertiesJson.createPhysicalProperties(), + swerveDriveJson.modules[i]); + } + SwerveDriveConfiguration swerveDriveConfiguration = + new SwerveDriveConfiguration( + moduleConfigurations, + swerveDriveJson.imu.createIMU(), + swerveDriveJson.invertedIMU, + driveFeedforward, + physicalPropertiesJson.createPhysicalProperties()); + + return new SwerveDrive( + swerveDriveConfiguration, + controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), + maxSpeed); + } +} diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java b/src/main/java/frc/robot/util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java new file mode 100644 index 00000000..614487af --- /dev/null +++ b/src/main/java/frc/robot/util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java @@ -0,0 +1,446 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright (c) 2024 FRC 3481 +// https://github.com/BroncBotz3481/YAGSL +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util.YAGSL_AzRBSI; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.motors.SwerveMotor; +import swervelib.parser.PIDFConfig; +import swervelib.telemetry.SwerveDriveTelemetry; + +/** {@link com.ctre.phoenix6.hardware.TalonFX} Swerve Motor. Made by Team 1466 WebbRobotics. */ +public class TalonFXSwerve_RBSI extends SwerveMotor { + + /** Wait time for status frames to show up. */ + public static double STATUS_TIMEOUT_SECONDS = 0.02; + + /** Factory default already occurred. */ + private final boolean factoryDefaultOccurred = false; + + /** Whether the absolute encoder is integrated. */ + private final boolean absoluteEncoder = false; + + /** Motion magic angle voltage setter. */ + private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); + + /** Velocity voltage setter for controlling drive motor. */ + private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); + + /** TalonFX motor controller. */ + TalonFX motor; + + /** Conversion factor for the motor. */ + private double conversionFactor; + + /** Current TalonFX configuration. */ + private TalonFXConfiguration configuration = new TalonFXConfiguration(); + + /** Current TalonFX Configurator. */ + private TalonFXConfigurator cfg; + + /** + * Constructor for TalonFX swerve motor. + * + * @param motor Motor to use. + * @param isDriveMotor Whether this motor is a drive motor. + */ + public TalonFXSwerve_RBSI(TalonFX motor, boolean isDriveMotor) { + this.isDriveMotor = isDriveMotor; + this.motor = motor; + this.cfg = motor.getConfigurator(); + + factoryDefaults(); + clearStickyFaults(); + + // if (SwerveDriveTelemetry.isSimulation) + // { + //// PhysicsSim.getInstance().addTalonFX(motor, .25, 6800); + // } + } + + /** + * Construct the TalonFX swerve motor given the ID and CANBus. + * + * @param id ID of the TalonFX on the CANBus. + * @param canbus CANBus on which the TalonFX is on. + * @param isDriveMotor Whether the motor is a drive or steering motor. + */ + public TalonFXSwerve_RBSI(int id, String canbus, boolean isDriveMotor) { + this(new TalonFX(id, canbus), isDriveMotor); + } + + /** + * Construct the TalonFX swerve motor given the ID. + * + * @param id ID of the TalonFX on the canbus. + * @param isDriveMotor Whether the motor is a drive or steering motor. + */ + public TalonFXSwerve_RBSI(int id, boolean isDriveMotor) { + this(new TalonFX(id), isDriveMotor); + } + + /** Configure the factory defaults. */ + @Override + public void factoryDefaults() { + if (!factoryDefaultOccurred) { + configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; + configuration.ClosedLoopGeneral.ContinuousWrap = true; + cfg.apply(configuration); + + m_angleVoltageSetter.UpdateFreqHz = 0; + // m_angleVoltageExpoSetter.UpdateFreqHz = 0; + m_velocityVoltageSetter.UpdateFreqHz = 0; + // motor.configFactoryDefault(); + // motor.setSensorPhase(true); + // motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); + // motor.configNeutralDeadband(0.001); + } + } + + /** Clear the sticky faults on the motor controller. */ + @Override + public void clearStickyFaults() { + motor.clearStickyFaults(); + } + + /** + * Set the absolute encoder to be a compatible absolute encoder. + * + * @param encoder The encoder to use. + */ + @Override + public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { + // Do not support. + return this; + } + + /** + * Configure the integrated encoder for the swerve module. Sets the conversion factors for + * position and velocity. + * + * @param positionConversionFactor The conversion factor to apply for position. + *


+ * Degrees:
+ * + * 360 / (angleGearRatio * encoderTicksPerRotation) + *
+ *


+ * Meters:
+ * + * (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation) + * + */ + @Override + public void configureIntegratedEncoder(double positionConversionFactor) { + cfg.refresh(configuration); + + positionConversionFactor = 1 / positionConversionFactor; + if (!isDriveMotor) { + positionConversionFactor *= 360; + } + conversionFactor = positionConversionFactor; + + configuration.MotionMagic = + configuration.MotionMagic.withMotionMagicCruiseVelocity(100.0 / positionConversionFactor) + .withMotionMagicAcceleration((100.0 / positionConversionFactor) / 0.100) + .withMotionMagicExpo_kV(0.12 * positionConversionFactor) + .withMotionMagicExpo_kA(0.1); + + configuration.Feedback.withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor) + .withSensorToMechanismRatio(positionConversionFactor); + + cfg.apply(configuration); + // Taken from democat's library. + // https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16 + configureCANStatusFrames(250); + } + + /** + * Set the CAN status frames. + * + * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information + */ + public void configureCANStatusFrames(int CANStatus1) { + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); + } + + /** + * Set the CAN status frames. + * + * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information + * @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed + * Supply Current Measurement, Sticky Fault Information + * @param CANStatus3 Quadrature Information + * @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature + * @param CANStatus8 Pulse Width Information + * @param CANStatus10 Motion Profiling/Motion Magic Information + * @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1) + * @param CANStatus13 PID0 (Primary PID) Information + * @param CANStatus14 PID1 (Auxiliary PID) Information + * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX) + * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current + * Measurement + */ + public void configureCANStatusFrames( + int CANStatus1, + int CANStatus2, + int CANStatus3, + int CANStatus4, + int CANStatus8, + int CANStatus10, + int CANStatus12, + int CANStatus13, + int CANStatus14, + int CANStatus21, + int CANStatusCurrent) { + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, + // CANStatusCurrent); + + // TODO: Configure Status Frame 2 thru 21 if necessary + // https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods + } + + /** + * Configure the PIDF values for the closed loop controller. 0 is disabled or off. + * + * @param config Configuration class holding the PIDF values. + */ + @Override + public void configurePIDF(PIDFConfig config) { + + cfg.refresh(configuration.Slot0); + cfg.apply( + configuration.Slot0.withKP(config.p).withKI(config.i).withKD(config.d).withKS(config.f)); + // configuration.slot0.integralZone = config.iz; + // configuration.slot0.closedLoopPeakOutput = config.output.max; + } + + /** + * Configure the PID wrapping for the position closed loop controller. + * + * @param minInput Minimum PID input. + * @param maxInput Maximum PID input. + */ + @Override + public void configurePIDWrapping(double minInput, double maxInput) { + cfg.refresh(configuration.ClosedLoopGeneral); + configuration.ClosedLoopGeneral.ContinuousWrap = true; + cfg.apply(configuration.ClosedLoopGeneral); + } + + /** + * Set the idle mode. + * + * @param isBrakeMode Set the brake mode. + */ + @Override + public void setMotorBrake(boolean isBrakeMode) { + motor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast); + } + + /** + * Set the motor to be inverted. + * + * @param inverted State of inversion. + */ + @Override + public void setInverted(boolean inverted) { + // Timer.delay(1); + motor.setInverted(inverted); + } + + /** Save the configurations from flash to EEPROM. */ + @Override + public void burnFlash() { + // Do nothing + } + + /** + * Set the percentage output. + * + * @param percentOutput percent out for the motor controller. + */ + @Override + public void set(double percentOutput) { + motor.set(percentOutput); + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in MPS or Angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + */ + @Override + public void setReference(double setpoint, double feedforward) { + setReference(setpoint, feedforward, getPosition()); + } + + /** + * Set the closed loop PID controller reference point. + * + * @param setpoint Setpoint in meters per second or angle in degrees. + * @param feedforward Feedforward in volt-meter-per-second or kV. + * @param position Only used on the angle motor, the position of the motor in degrees. + */ + @Override + public void setReference(double setpoint, double feedforward, double position) { + // if (SwerveDriveTelemetry.isSimulation) + // { + // PhysicsSim.getInstance().run(); + // } + + if (isDriveMotor) { + motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint).withFeedForward(feedforward)); + } else { + motor.setControl(m_angleVoltageSetter.withPosition(setpoint / 360.0)); + } + } + + /** + * Get the voltage output of the motor controller. + * + * @return Voltage output. + */ + @Override + public double getVoltage() { + return motor.getMotorVoltage().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(); + } + + /** + * Set the voltage of the motor. + * + * @param voltage Voltage to set. + */ + @Override + public void setVoltage(double voltage) { + motor.setVoltage(voltage); + } + + /** + * Get the applied dutycycle output. + * + * @return Applied dutycycle output to the motor. + */ + @Override + public double getAppliedOutput() { + return motor.getDutyCycle().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(); + } + + /** + * Get the velocity of the integrated encoder. + * + * @return velocity in Meters Per Second, or Degrees per Second. + */ + @Override + public double getVelocity() { + return motor.getVelocity().getValue(); + } + + /** + * Get the position of the integrated encoder. + * + * @return Position in Meters or Degrees. + */ + @Override + public double getPosition() { + return motor.getPosition().getValue(); + } + + /** + * Set the integrated encoder position. + * + * @param position Integrated encoder position. Should be angle in degrees or meters. + */ + @Override + public void setPosition(double position) { + if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) { + position = position < 0 ? (position % 360) + 360 : position; + cfg.setPosition(position / 360); + } + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + @Override + public void setVoltageCompensation(double nominalVoltage) { + // Do not implement + } + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in + * conjunction with voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + */ + @Override + public void setCurrentLimit(int currentLimit) { + cfg.refresh(configuration.CurrentLimits); + cfg.apply( + configuration.CurrentLimits.withStatorCurrentLimit(currentLimit) + .withStatorCurrentLimitEnable(true)); + } + + /** + * Set the maximum rate the open/closed loop output can change by. + * + * @param rampRate Time in seconds to go from 0 to full throttle. + */ + @Override + public void setLoopRampRate(double rampRate) { + cfg.refresh(configuration.ClosedLoopRamps); + cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate)); + } + + /** + * Get the motor object from the module. + * + * @return Motor object. + */ + @Override + public Object getMotor() { + return motor; + } + + /** + * Queries whether the absolute encoder is directly attached to the motor controller. + * + * @return connected absolute encoder state. + */ + @Override + public boolean isAttachedAbsoluteEncoder() { + return absoluteEncoder; + } +} From 587a0c8223f0a0606a4aef34113e86b8be4300d3 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 23 Oct 2024 12:06:32 -0700 Subject: [PATCH 30/57] Use Phoenix6 swervedrive for all-CTRE, YAGSL for all else? modified: README.md modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java modified: src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java modified: src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java modified: src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java new file: src/main/java/frc/robot/generated/TunerConstants.java new file: src/main/java/frc/robot/subsystems/swervedrive_phoenix/CommandSwerveDrivetrain.java renamed: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java -> src/main/java/frc/robot/subsystems/swervedrive_yagsl/SwerveSubsystem.java modified: src/main/java/frc/robot/util/YAGSL_AzRBSI/README modified: src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveParser_RBSI.java modified: src/main/java/frc/robot/util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java --- README.md | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- .../swervedrive/auto/AutoBalanceCommand.java | 2 +- .../swervedrive/drivebase/AbsoluteDrive.java | 2 +- .../drivebase/AbsoluteDriveAdv.java | 2 +- .../drivebase/AbsoluteFieldDrive.java | 2 +- .../frc/robot/generated/TunerConstants.java | 187 +++++++++++++++ .../CommandSwerveDrivetrain.java | 214 ++++++++++++++++++ .../SwerveSubsystem.java | 2 +- .../java/frc/robot/util/YAGSL_AzRBSI/README | 8 +- .../util/YAGSL_AzRBSI/SwerveParser_RBSI.java | 17 +- .../util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java | 25 +- 12 files changed, 436 insertions(+), 29 deletions(-) create mode 100644 src/main/java/frc/robot/generated/TunerConstants.java create mode 100644 src/main/java/frc/robot/subsystems/swervedrive_phoenix/CommandSwerveDrivetrain.java rename src/main/java/frc/robot/subsystems/{swervedrive => swervedrive_yagsl}/SwerveSubsystem.java (99%) diff --git a/README.md b/README.md index 333df17c..155690b6 100644 --- a/README.md +++ b/README.md @@ -36,6 +36,6 @@ effective logging for troubleshooting. * [WPILib](https://docs.wpilib.org/en/stable/index.html) -- FIRST basic libraries * [AdvantageKit](https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/docs/WHAT-IS-ADVANTAGEKIT.md) -- Logging -* [YAGSL](https://yagsl.gitbook.io/yagsl) -- Swerve drive library +* [YAGSL](https://yagsl.gitbook.io/yagsl) / [CTRE Phoenix6](https://v6.docs.ctr-electronics.com/en/stable/docs/api-reference/mechanisms/swerve/swerve-overview.html) -- Swerve drive library * [PathPlanner](https://pathplanner.dev/home.html) / [Choreo](https://sleipnirgroup.github.io/Choreo/) -- Autonomous path planning * [PhotonVision](https://docs.photonvision.org/en/latest/) -- Robot vision / tracking diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f5e8e2ff..66bef046 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,7 +36,7 @@ import frc.robot.subsystems.flywheel_example.Flywheel; import frc.robot.subsystems.flywheel_example.FlywheelIO; import frc.robot.subsystems.flywheel_example.FlywheelIOSim; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.subsystems.swervedrive_yagsl.SwerveSubsystem; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhoton; diff --git a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java b/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java index 27181671..acd8c928 100644 --- a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java +++ b/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java @@ -24,7 +24,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.subsystems.swervedrive_yagsl.SwerveSubsystem; /** * Auto Balance command using a simple PID controller. Created by Team 3512 SignalLogger.writeString("state", state.toString())), + new SysIdRoutine.Mechanism( + (volts) -> setControl(TranslationCharacterization.withVolts(volts)), null, this)); + + private final SysIdRoutine SysIdRoutineRotation = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + Volts.of(4), + null, + (state) -> SignalLogger.writeString("state", state.toString())), + new SysIdRoutine.Mechanism( + (volts) -> setControl(RotationCharacterization.withVolts(volts)), null, this)); + private final SysIdRoutine SysIdRoutineSteer = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + Volts.of(7), + null, + (state) -> SignalLogger.writeString("state", state.toString())), + new SysIdRoutine.Mechanism( + (volts) -> setControl(SteerCharacterization.withVolts(volts)), null, this)); + + /* Change this to the sysid routine you want to test */ + private final SysIdRoutine RoutineToApply = SysIdRoutineTranslation; + + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants driveTrainConstants, + double OdometryUpdateFrequency, + SwerveModuleConstants... modules) { + super(driveTrainConstants, OdometryUpdateFrequency, modules); + configurePathPlanner(); + if (Utils.isSimulation()) { + startSimThread(); + } + } + + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { + super(driveTrainConstants, modules); + configurePathPlanner(); + if (Utils.isSimulation()) { + startSimThread(); + } + } + + private void configurePathPlanner() { + double driveBaseRadius = 0; + for (var moduleLocation : m_moduleLocations) { + driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm()); + } + + AutoBuilder.configureHolonomic( + () -> this.getState().Pose, // Supplier of current robot pose + this::seedFieldRelative, // Consumer for seeding pose against auto + this::getCurrentRobotChassisSpeeds, + (speeds) -> + this.setControl( + AutoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot + new HolonomicPathFollowerConfig( + new PIDConstants(10, 0, 0), + new PIDConstants(10, 0, 0), + TunerConstants.kSpeedAt12VoltsMps, + driveBaseRadius, + new ReplanningConfig()), + () -> + DriverStation.getAlliance().orElse(Alliance.Blue) + == Alliance + .Red, // Assume the path needs to be flipped for Red vs Blue, this is normally + // the case + this); // Subsystem for requirements + } + + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } + + public Command getAutoPath(String pathName) { + return new PathPlannerAuto(pathName); + } + + /* + * Both the sysid commands are specific to one particular sysid routine, change + * which one you're trying to characterize + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return RoutineToApply.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return RoutineToApply.dynamic(direction); + } + + public ChassisSpeeds getCurrentRobotChassisSpeeds() { + return m_kinematics.toChassisSpeeds(getState().ModuleStates); + } + + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = + new Notifier( + () -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } + + @Override + public void periodic() { + /* Periodically try to apply the operator perspective */ + /* If we haven't applied the operator perspective before, then we should apply it regardless of DS state */ + /* This allows us to correct the perspective in case the robot code restarts mid-match */ + /* Otherwise, only check and apply the operator perspective if the DS is disabled */ + /* This ensures driving behavior doesn't change until an explicit disable event occurs during testing*/ + if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance() + .ifPresent( + (allianceColor) -> { + this.setOperatorPerspectiveForward( + allianceColor == Alliance.Red + ? RedAlliancePerspectiveRotation + : BlueAlliancePerspectiveRotation); + hasAppliedOperatorPerspective = true; + }); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive_yagsl/SwerveSubsystem.java similarity index 99% rename from src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java rename to src/main/java/frc/robot/subsystems/swervedrive_yagsl/SwerveSubsystem.java index d2184de7..519414cb 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive_yagsl/SwerveSubsystem.java @@ -17,7 +17,7 @@ // // NOTE: This module based on the YAGSL Example Project -package frc.robot.subsystems.swervedrive; +package frc.robot.subsystems.swervedrive_yagsl; import static frc.robot.Constants.DrivebaseConstants.*; diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/README b/src/main/java/frc/robot/util/YAGSL_AzRBSI/README index 18411457..356c7565 100644 --- a/src/main/java/frc/robot/util/YAGSL_AzRBSI/README +++ b/src/main/java/frc/robot/util/YAGSL_AzRBSI/README @@ -11,9 +11,9 @@ NOTE for developers: it will be necessary, from time to time, to merge in any modifications to these files from the main YAGSL repo to maintain compliant functionality with the remainder of YAGSL. -::: - -Last merged from YAGSL 21-Oct-2024, v2024.6.1.0 - ----------------- 21-Oct-2024, TPEB + +::: +HISTORY: + - Merged from YAGSL 21-Oct-2024, v2024.6.1.0, TPEB diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveParser_RBSI.java b/src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveParser_RBSI.java index d899f876..4bfcbaa6 100644 --- a/src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveParser_RBSI.java +++ b/src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveParser_RBSI.java @@ -15,7 +15,6 @@ package frc.robot.util.YAGSL_AzRBSI; -import com.fasterxml.jackson.databind.JsonNode; import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import java.io.File; @@ -37,7 +36,7 @@ public class SwerveParser_RBSI { /** Module number mapped to the JSON name. */ private static final HashMap moduleConfigs = new HashMap<>(); - /** Parsed swervedrive.json */ + /** Parsed swervedrive.json -- use the RBSI version to catch CTRE Pro */ public static SwerveDriveJson_RBSI swerveDriveJson; /** Parsed controllerproperties.json */ @@ -97,20 +96,6 @@ public static SwerveModule getModuleConfigurationByName( return driveConfiguration.modules[moduleConfigs.get(name + ".json")]; } - /** - * Open JSON file. - * - * @param file JSON File to open. - * @return JsonNode of file. - */ - private JsonNode openJson(File file) { - try { - return new ObjectMapper().readTree(file); - } catch (IOException e) { - throw new RuntimeException(e); - } - } - /** * Check directory structure. * diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java b/src/main/java/frc/robot/util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java index 614487af..34503da7 100644 --- a/src/main/java/frc/robot/util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java +++ b/src/main/java/frc/robot/util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java @@ -91,7 +91,7 @@ public TalonFXSwerve_RBSI(int id, String canbus, boolean isDriveMotor) { /** * Construct the TalonFX swerve motor given the ID. * - * @param id ID of the TalonFX on the canbus. + * @param id ID of the TalonFX on the RIO canbus. * @param isDriveMotor Whether the motor is a drive or steering motor. */ public TalonFXSwerve_RBSI(int id, boolean isDriveMotor) { @@ -129,7 +129,28 @@ public void clearStickyFaults() { */ @Override public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { - // Do not support. + + // if (encoder == null) + // { + // absoluteEncoder = null; + // configureSparkMax(() -> pid.setFeedbackDevice(this.encoder)); + // velocity = this.encoder::getVelocity; + // position = this.encoder::getPosition; + // } else if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) + // { + // DriverStation.reportWarning( + // "IF possible configure the encoder offset in the REV Hardware Client instead of using + // the" + + // " absoluteEncoderOffset in the Swerve Module JSON!", + // false); + // absoluteEncoder = encoder; + // configureSparkMax(() -> pid.setFeedbackDevice((MotorFeedbackSensor) + // absoluteEncoder.getAbsoluteEncoder())); + // velocity = absoluteEncoder::getVelocity; + // position = absoluteEncoder::getAbsolutePosition; + + // } + return this; } From 36c0dcb21397041349ffcf8487985130d3a3f963 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 23 Oct 2024 15:11:28 -0700 Subject: [PATCH 31/57] Stopoff on the way toward CTRE-centric swerve modified: README.md modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java modified: src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java modified: src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java modified: src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java new file: src/main/java/frc/robot/generated/README modified: src/main/java/frc/robot/generated/TunerConstants.java new file: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java renamed: src/main/java/frc/robot/subsystems/swervedrive_phoenix/CommandSwerveDrivetrain.java -> src/main/java/frc/robot/subsystems/swervedrive/underlying/Phoenix6Swerve.java renamed: src/main/java/frc/robot/subsystems/swervedrive_yagsl/SwerveSubsystem.java -> src/main/java/frc/robot/subsystems/swervedrive/underlying/YAGSLSwerve.java deleted: src/main/java/frc/robot/util/YAGSL_AzRBSI/CANCoderSwerve_RBSI.java deleted: src/main/java/frc/robot/util/YAGSL_AzRBSI/DeviceJson_RBSI.java deleted: src/main/java/frc/robot/util/YAGSL_AzRBSI/Pigeon2Swerve_RBSI.java deleted: src/main/java/frc/robot/util/YAGSL_AzRBSI/README deleted: src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveDriveJson_RBSI.java deleted: src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveParser_RBSI.java deleted: src/main/java/frc/robot/util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java --- README.md | 2 +- src/main/java/frc/robot/Constants.java | 15 +- src/main/java/frc/robot/RobotContainer.java | 10 +- .../swervedrive/auto/AutoBalanceCommand.java | 6 +- .../swervedrive/drivebase/AbsoluteDrive.java | 6 +- .../drivebase/AbsoluteDriveAdv.java | 6 +- .../drivebase/AbsoluteFieldDrive.java | 6 +- src/main/java/frc/robot/generated/README | 2 + .../frc/robot/generated/TunerConstants.java | 6 +- .../swervedrive/SwerveSubsystem.java | 55 +++ .../underlying/Phoenix6Swerve.java} | 8 +- .../underlying/YAGSLSwerve.java} | 14 +- .../YAGSL_AzRBSI/CANCoderSwerve_RBSI.java | 210 -------- .../util/YAGSL_AzRBSI/DeviceJson_RBSI.java | 201 -------- .../util/YAGSL_AzRBSI/Pigeon2Swerve_RBSI.java | 161 ------ .../java/frc/robot/util/YAGSL_AzRBSI/README | 19 - .../YAGSL_AzRBSI/SwerveDriveJson_RBSI.java | 32 -- .../util/YAGSL_AzRBSI/SwerveParser_RBSI.java | 214 -------- .../util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java | 467 ------------------ 19 files changed, 102 insertions(+), 1338 deletions(-) create mode 100644 src/main/java/frc/robot/generated/README create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java rename src/main/java/frc/robot/subsystems/{swervedrive_phoenix/CommandSwerveDrivetrain.java => swervedrive/underlying/Phoenix6Swerve.java} (97%) rename src/main/java/frc/robot/subsystems/{swervedrive_yagsl/SwerveSubsystem.java => swervedrive/underlying/YAGSLSwerve.java} (98%) delete mode 100644 src/main/java/frc/robot/util/YAGSL_AzRBSI/CANCoderSwerve_RBSI.java delete mode 100644 src/main/java/frc/robot/util/YAGSL_AzRBSI/DeviceJson_RBSI.java delete mode 100644 src/main/java/frc/robot/util/YAGSL_AzRBSI/Pigeon2Swerve_RBSI.java delete mode 100644 src/main/java/frc/robot/util/YAGSL_AzRBSI/README delete mode 100644 src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveDriveJson_RBSI.java delete mode 100644 src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveParser_RBSI.java delete mode 100644 src/main/java/frc/robot/util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java diff --git a/README.md b/README.md index 155690b6..a0d734dd 100644 --- a/README.md +++ b/README.md @@ -36,6 +36,6 @@ effective logging for troubleshooting. * [WPILib](https://docs.wpilib.org/en/stable/index.html) -- FIRST basic libraries * [AdvantageKit](https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/docs/WHAT-IS-ADVANTAGEKIT.md) -- Logging -* [YAGSL](https://yagsl.gitbook.io/yagsl) / [CTRE Phoenix6](https://v6.docs.ctr-electronics.com/en/stable/docs/api-reference/mechanisms/swerve/swerve-overview.html) -- Swerve drive library +* [CTRE Phoenix6](https://v6.docs.ctr-electronics.com/en/stable/docs/api-reference/mechanisms/swerve/swerve-overview.html) / [YAGSL](https://yagsl.gitbook.io/yagsl) -- Swerve drive library * [PathPlanner](https://pathplanner.dev/home.html) / [Choreo](https://sleipnirgroup.github.io/Choreo/) -- Autonomous path planning * [PhotonVision](https://docs.photonvision.org/en/latest/) -- Robot vision / tracking diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e6e88765..578bc871 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -51,6 +51,8 @@ public final class Constants { */ private static RobotType robotType = RobotType.SIMBOT; + private static SwerveType swerveType = SwerveType.PHOENIX6; + public static boolean disableHAL = false; /** Enumerate the robot types (add additional bots here) */ @@ -98,6 +100,17 @@ public static void main(String... args) { } } + /** Enumerate the supported swerve drive types */ + public static enum SwerveType { + YAGSL, // The generic YAGSL swerve library + PHOENIX6 // The all-CTRE Phoenix6 swerve library + } + + /** Get the current swerve drive type */ + public static SwerveType getSwerve() { + return swerveType; + } + /***************************************************************************/ /* The remainder of this file contains physical and/or software constants for the various subsystems of the robot */ @@ -106,7 +119,7 @@ public static void main(String... args) { public static final boolean tuningMode = false; - /** Physical Constants for Robot Operation ************ */ + /** Physical Constants for Robot Operation ******************************* */ public static final class PhysicalConstants { public static final double kRobotMass = (148 - 20.3) * 0.453592; // 32lbs * kg per pound public static final Matter kChassis = diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 66bef046..f479002c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,7 +23,6 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -36,13 +35,12 @@ import frc.robot.subsystems.flywheel_example.Flywheel; import frc.robot.subsystems.flywheel_example.FlywheelIO; import frc.robot.subsystems.flywheel_example.FlywheelIOSim; -import frc.robot.subsystems.swervedrive_yagsl.SwerveSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhoton; import frc.robot.util.CanDeviceId; import frc.robot.util.OverrideSwitches; -import java.io.File; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class RobotContainer { @@ -78,7 +76,7 @@ public RobotContainer() { case REAL: // Real robot, instantiate hardware IO implementations // YAGSL drivebase, get config from deploy directory - m_drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); + m_drivebase = new SwerveSubsystem(Constants.getSwerve()); m_flywheel = new Flywheel(new FlywheelIOSim()); // new Flywheel(new FlywheelIOTalonFX()); m_vision = new Vision( @@ -88,14 +86,14 @@ public RobotContainer() { case SIM: // Sim robot, instantiate physics sim IO implementations - m_drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); + m_drivebase = new SwerveSubsystem(Constants.getSwerve()); m_flywheel = new Flywheel(new FlywheelIOSim()); m_vision = new Vision(this::getAprilTagLayoutType); break; default: // Replayed robot, disable IO implementations - m_drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); + m_drivebase = new SwerveSubsystem(Constants.getSwerve()); m_flywheel = new Flywheel(new FlywheelIO() {}); m_vision = new Vision(this::getAprilTagLayoutType, new VisionIO() {}, new VisionIO() {}); break; diff --git a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java b/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java index acd8c928..013021c7 100644 --- a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java +++ b/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java @@ -24,7 +24,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.swervedrive_yagsl.SwerveSubsystem; +import frc.robot.subsystems.swervedrive.underlying.YAGSLSwerve; /** * Auto Balance command using a simple PID controller. Created by Team 3512 swerveType) { + + if (swerveType == SwerveType.YAGSL) { + + // Load up the YASGL things into this class + y_swerve = new YAGSLSwerve(new File(Filesystem.getDeployDirectory(), "swerve")); + + } else if (swerveType == SwerveType.PHOENIX6) { + + // Load up the Phoenix6 things into this class + p_swerve = TunerConstants.DriveTrain; + + } else { + + // Raise an error because something unknown was passed + + } + } + + // +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive_phoenix/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervedrive/underlying/Phoenix6Swerve.java similarity index 97% rename from src/main/java/frc/robot/subsystems/swervedrive_phoenix/CommandSwerveDrivetrain.java rename to src/main/java/frc/robot/subsystems/swervedrive/underlying/Phoenix6Swerve.java index 120ed995..95e22f64 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive_phoenix/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/underlying/Phoenix6Swerve.java @@ -18,7 +18,7 @@ // NOTE: This module based on the CTRE Phoenix6 examples // https://github.com/CrossTheRoadElec/Phoenix6-Examples -package frc.robot.subsystems.swervedrive_phoenix; +package frc.robot.subsystems.swervedrive.underlying; import static edu.wpi.first.units.Units.Volts; @@ -49,7 +49,7 @@ * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used * in command-based projects easily. */ -public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { +public class Phoenix6Swerve extends SwerveDrivetrain implements Subsystem { private static final double kSimLoopPeriod = 0.005; // 5 ms private Notifier m_simNotifier = null; private double m_lastSimTime; @@ -104,7 +104,7 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst /* Change this to the sysid routine you want to test */ private final SysIdRoutine RoutineToApply = SysIdRoutineTranslation; - public CommandSwerveDrivetrain( + public Phoenix6Swerve( SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) { @@ -115,7 +115,7 @@ public CommandSwerveDrivetrain( } } - public CommandSwerveDrivetrain( + public Phoenix6Swerve( SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { super(driveTrainConstants, modules); configurePathPlanner(); diff --git a/src/main/java/frc/robot/subsystems/swervedrive_yagsl/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/underlying/YAGSLSwerve.java similarity index 98% rename from src/main/java/frc/robot/subsystems/swervedrive_yagsl/SwerveSubsystem.java rename to src/main/java/frc/robot/subsystems/swervedrive/underlying/YAGSLSwerve.java index 519414cb..b502c758 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive_yagsl/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/underlying/YAGSLSwerve.java @@ -17,7 +17,7 @@ // // NOTE: This module based on the YAGSL Example Project -package frc.robot.subsystems.swervedrive_yagsl; +package frc.robot.subsystems.swervedrive.underlying; import static frc.robot.Constants.DrivebaseConstants.*; @@ -43,7 +43,6 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.Constants.AutonConstants; import frc.robot.subsystems.vision.Vision; -import frc.robot.util.YAGSL_AzRBSI.SwerveParser_RBSI; import java.io.File; import java.util.Arrays; import java.util.function.DoubleSupplier; @@ -57,11 +56,12 @@ import swervelib.math.SwerveMath; import swervelib.parser.SwerveControllerConfiguration; import swervelib.parser.SwerveDriveConfiguration; +import swervelib.parser.SwerveParser; import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; -/** SwerveSubsystem module */ -public class SwerveSubsystem extends SubsystemBase { +/** YAGSLSwerve module */ +public class YAGSLSwerve extends SubsystemBase { /** Swerve drive object */ private final SwerveDrive swerveDrive; @@ -77,7 +77,7 @@ public class SwerveSubsystem extends SubsystemBase { * * @param directory Directory of swerve drive config files. */ - public SwerveSubsystem(File directory) { + public YAGSLSwerve(File directory) { // Angle conversion factor is 360 / (GEAR RATIO * ENCODER RESOLUTION) double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(TURN_GEAR_RATIO); @@ -99,7 +99,7 @@ public SwerveSubsystem(File directory) { // created. SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; try { - swerveDrive = new SwerveParser_RBSI(directory).createSwerveDrive(MAX_LINEAR_SPEED); + swerveDrive = new SwerveParser(directory).createSwerveDrive(MAX_LINEAR_SPEED); // Alternative method if you don't want to supply the conversion factor via JSON files. // swerveDrive = new SwerveParser_RBSI(directory).createSwerveDrive(maximumSpeed, // angleConversionFactor, driveConversionFactor); @@ -140,7 +140,7 @@ public SwerveSubsystem(File directory) { * @param driveCfg SwerveDriveConfiguration for the swerve. * @param controllerCfg Swerve Controller. */ - public SwerveSubsystem( + public YAGSLSwerve( SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { swerveDrive = new SwerveDrive(driveCfg, controllerCfg, MAX_LINEAR_SPEED); } diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/CANCoderSwerve_RBSI.java b/src/main/java/frc/robot/util/YAGSL_AzRBSI/CANCoderSwerve_RBSI.java deleted file mode 100644 index e45393b6..00000000 --- a/src/main/java/frc/robot/util/YAGSL_AzRBSI/CANCoderSwerve_RBSI.java +++ /dev/null @@ -1,210 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2024 FRC 3481 -// https://github.com/BroncBotz3481/YAGSL -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.util.YAGSL_AzRBSI; - -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.CANcoderConfigurator; -import com.ctre.phoenix6.configs.MagnetSensorConfigs; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; -import com.ctre.phoenix6.signals.MagnetHealthValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import swervelib.encoders.SwerveAbsoluteEncoder; -import swervelib.telemetry.Alert; - -/** Swerve Absolute Encoder for CTRE CANCoders. */ -public class CANCoderSwerve_RBSI extends SwerveAbsoluteEncoder { - - /** Wait time for status frames to show up. */ - public static double STATUS_TIMEOUT_SECONDS = 0.02; - - /** CANCoder with WPILib sendable and support. */ - public CANcoder encoder; - - /** An {@link Alert} for if the CANCoder magnet field is less than ideal. */ - private Alert magnetFieldLessThanIdeal; - - /** An {@link Alert} for if the CANCoder reading is faulty. */ - private Alert readingFaulty; - - /** An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored. */ - private Alert readingIgnored; - - /** An {@link Alert} for if the absolute encoder offset cannot be set. */ - private Alert cannotSetOffset; - - /** - * Initialize the CANCoder on the standard CANBus. - * - * @param id CAN ID. - */ - public CANCoderSwerve_RBSI(int id) { - // Empty string uses the default canbus for the system - this(id, ""); - } - - /** - * Initialize the CANCoder on the CANivore. - * - * @param id CAN ID. - * @param canbus CAN bus to initialize it on. - */ - public CANCoderSwerve_RBSI(int id, String canbus) { - encoder = new CANcoder(id, canbus); - magnetFieldLessThanIdeal = - new Alert( - "Encoders", - "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.", - Alert.AlertType.WARNING); - readingFaulty = - new Alert( - "Encoders", - "CANCoder " + encoder.getDeviceID() + " reading was faulty.", - Alert.AlertType.WARNING); - readingIgnored = - new Alert( - "Encoders", - "CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.", - Alert.AlertType.WARNING); - cannotSetOffset = - new Alert( - "Encoders", - "Failure to set CANCoder " + encoder.getDeviceID() + " Absolute Encoder Offset", - Alert.AlertType.WARNING); - } - - /** Reset the encoder to factory defaults. */ - @Override - public void factoryDefault() { - encoder.getConfigurator().apply(new CANcoderConfiguration()); - } - - /** Clear sticky faults on the encoder. */ - @Override - public void clearStickyFaults() { - encoder.clearStickyFaults(); - } - - /** - * Configure the absolute encoder to read from [0, 360) per second. - * - * @param inverted Whether the encoder is inverted. - */ - @Override - public void configure(boolean inverted) { - CANcoderConfigurator cfg = encoder.getConfigurator(); - MagnetSensorConfigs magnetSensorConfiguration = new MagnetSensorConfigs(); - cfg.refresh(magnetSensorConfiguration); - cfg.apply( - magnetSensorConfiguration - .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1) - .withSensorDirection( - inverted - ? SensorDirectionValue.Clockwise_Positive - : SensorDirectionValue.CounterClockwise_Positive)); - } - - /** - * Get the absolute position of the encoder. Sets {@link SwerveAbsoluteEncoder#readingError} on - * erroneous readings. - * - * @return Absolute position in degrees from [0, 360). - */ - @Override - public double getAbsolutePosition() { - readingError = false; - MagnetHealthValue strength = encoder.getMagnetHealth().getValue(); - - magnetFieldLessThanIdeal.set(strength != MagnetHealthValue.Magnet_Green); - if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red) { - readingError = true; - readingFaulty.set(true); - return 0; - } else { - readingFaulty.set(false); - } - - StatusSignal angle = encoder.getAbsolutePosition(); - - // Taken from democat's library. - // Source: - // https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74 - for (int i = 0; i < maximumRetries; i++) { - if (angle.getStatus() == StatusCode.OK) { - break; - } - angle = angle.waitForUpdate(STATUS_TIMEOUT_SECONDS); - } - if (angle.getStatus() != StatusCode.OK) { - readingError = true; - readingIgnored.set(true); - } else { - readingIgnored.set(false); - } - - return angle.getValue() * 360; - } - - /** - * Get the instantiated absolute encoder Object. - * - * @return Absolute encoder object. - */ - @Override - public Object getAbsoluteEncoder() { - return encoder; - } - - /** - * Sets the Absolute Encoder Offset within the CANcoder's Memory. - * - * @param offset the offset the Absolute Encoder uses as the zero point in degrees. - * @return if setting Absolute Encoder Offset was successful or not. - */ - @Override - public boolean setAbsoluteEncoderOffset(double offset) { - CANcoderConfigurator cfg = encoder.getConfigurator(); - MagnetSensorConfigs magCfg = new MagnetSensorConfigs(); - StatusCode error = cfg.refresh(magCfg); - if (error != StatusCode.OK) { - return false; - } - error = cfg.apply(magCfg.withMagnetOffset(offset / 360)); - cannotSetOffset.setText( - "Failure to set CANCoder " - + encoder.getDeviceID() - + " Absolute Encoder Offset Error: " - + error); - if (error == StatusCode.OK) { - cannotSetOffset.set(false); - return true; - } - cannotSetOffset.set(true); - return false; - } - - /** - * Get the velocity in degrees/sec. - * - * @return velocity in degrees/sec. - */ - @Override - public double getVelocity() { - return encoder.getVelocity().getValue() * 360; - } -} diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/DeviceJson_RBSI.java b/src/main/java/frc/robot/util/YAGSL_AzRBSI/DeviceJson_RBSI.java deleted file mode 100644 index 6df0c2e4..00000000 --- a/src/main/java/frc/robot/util/YAGSL_AzRBSI/DeviceJson_RBSI.java +++ /dev/null @@ -1,201 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2024 FRC 3481 -// https://github.com/BroncBotz3481/YAGSL -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.util.YAGSL_AzRBSI; - -import static swervelib.telemetry.SwerveDriveTelemetry.canIdWarning; -import static swervelib.telemetry.SwerveDriveTelemetry.i2cLockupWarning; -import static swervelib.telemetry.SwerveDriveTelemetry.serialCommsIssueWarning; - -import com.revrobotics.SparkRelativeEncoder.Type; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.SPI; -import edu.wpi.first.wpilibj.SerialPort.Port; -import swervelib.encoders.AnalogAbsoluteEncoderSwerve; -import swervelib.encoders.CanAndMagSwerve; -import swervelib.encoders.PWMDutyCycleEncoderSwerve; -import swervelib.encoders.SparkMaxAnalogEncoderSwerve; -import swervelib.encoders.SparkMaxEncoderSwerve; -import swervelib.encoders.SwerveAbsoluteEncoder; -import swervelib.imu.ADIS16448Swerve; -import swervelib.imu.ADIS16470Swerve; -import swervelib.imu.ADXRS450Swerve; -import swervelib.imu.AnalogGyroSwerve; -import swervelib.imu.CanandgyroSwerve; -import swervelib.imu.NavXSwerve; -import swervelib.imu.PigeonSwerve; -import swervelib.imu.SwerveIMU; -import swervelib.motors.SparkFlexSwerve; -import swervelib.motors.SparkMaxBrushedMotorSwerve; -import swervelib.motors.SparkMaxSwerve; -import swervelib.motors.SwerveMotor; -import swervelib.motors.TalonSRXSwerve; - -/** Device JSON parsed class. Used to access the JSON data. */ -public class DeviceJson_RBSI { - - /** The device type, e.g. pigeon/pigeon2/sparkmax/talonfx/navx */ - public String type; - - /** The CAN ID or pin ID of the device. */ - public int id; - - /** The CAN bus name which the device resides on if using CAN. */ - public String canbus = ""; - - /** - * Create a {@link SwerveAbsoluteEncoder} from the current configuration. - * - * @param motor {@link SwerveMotor} of which attached encoders will be created from, only used - * when the type is "attached" or "canandencoder". - * @return {@link SwerveAbsoluteEncoder} given. - */ - public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor) { - if (id > 40) { - canIdWarning.set(true); - } - switch (type) { - case "none": - return null; - case "integrated": - case "attached": - case "canandmag": - case "canandcoder": - return new SparkMaxEncoderSwerve(motor, 360); - case "sparkmax_analog": - return new SparkMaxAnalogEncoderSwerve(motor, 3.3); - case "sparkmax_analog5v": - return new SparkMaxAnalogEncoderSwerve(motor, 5); - case "canandcoder_can": - case "canandmag_can": - return new CanAndMagSwerve(id); - case "ctre_mag": - case "rev_hex": - case "throughbore": - case "am_mag": - case "dutycycle": - return new PWMDutyCycleEncoderSwerve(id); - case "thrifty": - case "ma3": - case "analog": - return new AnalogAbsoluteEncoderSwerve(id); - case "cancoder": - return new CANCoderSwerve_RBSI(id, canbus != null ? canbus : ""); - default: - throw new RuntimeException(type + " is not a recognized absolute encoder type."); - } - } - - /** - * Create a {@link SwerveIMU} from the given configuration. - * - * @return {@link SwerveIMU} given. - */ - public SwerveIMU createIMU() { - if (id > 40) { - canIdWarning.set(true); - } - switch (type) { - case "adis16448": - return new ADIS16448Swerve(); - case "adis16470": - return new ADIS16470Swerve(); - case "adxrs450": - return new ADXRS450Swerve(); - case "analog": - return new AnalogGyroSwerve(id); - case "canandgyro": - return new CanandgyroSwerve(id); - case "navx": - case "navx_spi": - return new NavXSwerve(SPI.Port.kMXP); - case "navx_i2c": - DriverStation.reportWarning( - "WARNING: There exists an I2C lockup issue on the roboRIO that could occur, more information here: " - + "\nhttps://docs.wpilib.org/en/stable/docs/yearly-overview/known-issues" - + ".html#onboard-i2c-causing-system-lockups", - false); - i2cLockupWarning.set(true); - return new NavXSwerve(I2C.Port.kMXP); - case "navx_usb": - DriverStation.reportWarning( - "WARNING: There is issues when using USB camera's and the NavX like this!\n" - + "https://pdocs.kauailabs.com/navx-mxp/guidance/selecting-an-interface/", - false); - serialCommsIssueWarning.set(true); - return new NavXSwerve(Port.kUSB); - case "navx_mxp_serial": - serialCommsIssueWarning.set(true); - return new NavXSwerve(Port.kMXP); - case "pigeon": - return new PigeonSwerve(id); - case "pigeon2": - return new Pigeon2Swerve_RBSI(id, canbus != null ? canbus : ""); - default: - throw new RuntimeException(type + " is not a recognized imu/gyroscope type."); - } - } - - /** - * Create a {@link SwerveMotor} from the given configuration. - * - * @param isDriveMotor If the motor being generated is a drive motor. - * @return {@link SwerveMotor} given. - */ - public SwerveMotor createMotor(boolean isDriveMotor) { - if (id > 40) { - canIdWarning.set(true); - } - switch (type) { - case "sparkmax_brushed": - switch (canbus) { - case "greyhill_63r256": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, false); - case "srx_mag_encoder": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, false); - case "throughbore": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 8192, false); - case "throughbore_dataport": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 8192, true); - case "greyhill_63r256_dataport": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, true); - case "srx_mag_encoder_dataport": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, true); - default: - if (isDriveMotor) { - throw new RuntimeException( - "Spark MAX " + id + " MUST have a encoder attached to the motor controller."); - } - // We are creating a motor for an angle motor which will use the absolute encoder - // attached to the data port. - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 0, false); - } - case "neo": - case "sparkmax": - return new SparkMaxSwerve(id, isDriveMotor); - case "sparkflex": - return new SparkFlexSwerve(id, isDriveMotor); - case "falcon": - case "kraken": - case "talonfx": - return new TalonFXSwerve_RBSI(id, canbus != null ? canbus : "", isDriveMotor); - case "talonsrx": - return new TalonSRXSwerve(id, isDriveMotor); - default: - throw new RuntimeException(type + " is not a recognized motor type."); - } - } -} diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/Pigeon2Swerve_RBSI.java b/src/main/java/frc/robot/util/YAGSL_AzRBSI/Pigeon2Swerve_RBSI.java deleted file mode 100644 index 0b54ff60..00000000 --- a/src/main/java/frc/robot/util/YAGSL_AzRBSI/Pigeon2Swerve_RBSI.java +++ /dev/null @@ -1,161 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2024 FRC 3481 -// https://github.com/BroncBotz3481/YAGSL -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.util.YAGSL_AzRBSI; - -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Pigeon2Configuration; -import com.ctre.phoenix6.configs.Pigeon2Configurator; -import com.ctre.phoenix6.hardware.Pigeon2; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.Optional; -import swervelib.imu.SwerveIMU; - -/** SwerveIMU interface for the Pigeon2 */ -public class Pigeon2Swerve_RBSI extends SwerveIMU { - - /** Wait time for status frames to show up. */ - public static double STATUS_TIMEOUT_SECONDS = 0.04; - - /** Pigeon2 IMU device. */ - Pigeon2 imu; - - /** Offset for the Pigeon 2. */ - private Rotation3d offset = new Rotation3d(); - - /** Inversion for the gyro */ - private boolean invertedIMU = false; - - /** Pigeon2 configurator. */ - private Pigeon2Configurator cfg; - - /** - * Generate the SwerveIMU for pigeon. - * - * @param canid CAN ID for the pigeon - * @param canbus CAN Bus name the pigeon resides on. - */ - public Pigeon2Swerve_RBSI(int canid, String canbus) { - imu = new Pigeon2(canid, canbus); - this.cfg = imu.getConfigurator(); - SmartDashboard.putData(imu); - } - - /** - * Generate the SwerveIMU for pigeon. - * - * @param canid CAN ID for the pigeon - */ - public Pigeon2Swerve_RBSI(int canid) { - this(canid, ""); - } - - /** Reset IMU to factory default. */ - @Override - public void factoryDefault() { - Pigeon2Configuration config = new Pigeon2Configuration(); - - // Compass utilization causes readings to jump dramatically in some cases. - cfg.apply(config.Pigeon2Features.withEnableCompass(false)); - } - - /** Clear sticky faults on IMU. */ - @Override - public void clearStickyFaults() { - imu.clearStickyFaults(); - } - - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public void setOffset(Rotation3d offset) { - this.offset = offset; - } - - /** - * Set the gyro to invert its default direction - * - * @param invertIMU invert gyro direction - */ - public void setInverted(boolean invertIMU) { - invertedIMU = invertIMU; - } - - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRawRotation3d() { - Rotation3d reading = imu.getRotation3d(); - return invertedIMU ? reading.unaryMinus() : reading; - } - - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRotation3d() { - return getRawRotation3d().minus(offset); - } - - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration - * isn't supported returns empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - @Override - public Optional getAccel() { - // TODO: Switch to suppliers. - StatusSignal xAcc = imu.getAccelerationX(); - StatusSignal yAcc = imu.getAccelerationY(); - StatusSignal zAcc = imu.getAccelerationZ(); - - return Optional.of( - new Translation3d( - xAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(), - yAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(), - zAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue()) - .times(9.81 / 16384.0)); - } - - /** - * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported - * returns empty. - * - * @return {@link Double} of the rotation rate as an {@link Optional}. - */ - public double getRate() { - return imu.getRate(); - } - - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - @Override - public Object getIMU() { - return imu; - } -} diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/README b/src/main/java/frc/robot/util/YAGSL_AzRBSI/README deleted file mode 100644 index 356c7565..00000000 --- a/src/main/java/frc/robot/util/YAGSL_AzRBSI/README +++ /dev/null @@ -1,19 +0,0 @@ -This directory contains modified versions of various routines from the YAGSL -library (https://github.com/BroncBotz3481/YAGSL). The purpose of the modified -routines is to allow access to the CRTE Phoenix Pro features on supported -devices from within YAGSL -- which otherwise ignores Phoenix Pro. - -Since the Az-RBSI is designed around a CTRE/SDS swerve base, yet we wish to -make use of the features of the YAGSL, it is necessary to extend these -particular routines to allow for Phoenix Pro features (if enabled / licensed). - -NOTE for developers: it will be necessary, from time to time, to merge in any -modifications to these files from the main YAGSL repo to maintain compliant -functionality with the remainder of YAGSL. - ------------------ -21-Oct-2024, TPEB - -::: -HISTORY: - - Merged from YAGSL 21-Oct-2024, v2024.6.1.0, TPEB diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveDriveJson_RBSI.java b/src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveDriveJson_RBSI.java deleted file mode 100644 index f06ab4f3..00000000 --- a/src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveDriveJson_RBSI.java +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2024 FRC 3481 -// https://github.com/BroncBotz3481/YAGSL -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.util.YAGSL_AzRBSI; - -/** - * {@link swervelib.SwerveDrive} JSON parsed class. Used to access parsed data from the - * swervedrive.json file. - */ -public class SwerveDriveJson_RBSI { - - /** Robot IMU used to determine heading of the robot. */ - public DeviceJson_RBSI imu; - - /** Invert the IMU of the robot. */ - public boolean invertedIMU; - - /** Module JSONs in order clockwise order starting from front left. */ - public String[] modules; -} diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveParser_RBSI.java b/src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveParser_RBSI.java deleted file mode 100644 index 4bfcbaa6..00000000 --- a/src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveParser_RBSI.java +++ /dev/null @@ -1,214 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2024 FRC 3481 -// https://github.com/BroncBotz3481/YAGSL -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.util.YAGSL_AzRBSI; - -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import java.io.File; -import java.io.IOException; -import java.util.HashMap; -import swervelib.SwerveDrive; -import swervelib.SwerveModule; -import swervelib.math.SwerveMath; -import swervelib.parser.SwerveDriveConfiguration; -import swervelib.parser.SwerveModuleConfiguration; -import swervelib.parser.json.ControllerPropertiesJson; -import swervelib.parser.json.ModuleJson; -import swervelib.parser.json.PIDFPropertiesJson; -import swervelib.parser.json.PhysicalPropertiesJson; - -/** Helper class used to parse the JSON directory with specified configuration options. */ -public class SwerveParser_RBSI { - - /** Module number mapped to the JSON name. */ - private static final HashMap moduleConfigs = new HashMap<>(); - - /** Parsed swervedrive.json -- use the RBSI version to catch CTRE Pro */ - public static SwerveDriveJson_RBSI swerveDriveJson; - - /** Parsed controllerproperties.json */ - public static ControllerPropertiesJson controllerPropertiesJson; - - /** Parsed modules/pidfproperties.json */ - public static PIDFPropertiesJson pidfPropertiesJson; - - /** Parsed modules/physicalproperties.json */ - public static PhysicalPropertiesJson physicalPropertiesJson; - - /** Array holding the module jsons given in {@link SwerveDriveJson_RBSI}. */ - public static ModuleJson[] moduleJsons; - - /** - * Construct a swerve parser. Will throw an error if there is a missing file. - * - * @param directory Directory with swerve configurations. - * @throws IOException if a file doesn't exist. - */ - public SwerveParser_RBSI(File directory) throws IOException { - checkDirectory(directory); - swerveDriveJson = - new ObjectMapper() - .readValue(new File(directory, "swervedrive.json"), SwerveDriveJson_RBSI.class); - controllerPropertiesJson = - new ObjectMapper() - .readValue( - new File(directory, "controllerproperties.json"), ControllerPropertiesJson.class); - pidfPropertiesJson = - new ObjectMapper() - .readValue( - new File(directory, "modules/pidfproperties.json"), PIDFPropertiesJson.class); - physicalPropertiesJson = - new ObjectMapper() - .readValue( - new File(directory, "modules/physicalproperties.json"), - PhysicalPropertiesJson.class); - moduleJsons = new ModuleJson[swerveDriveJson.modules.length]; - for (int i = 0; i < moduleJsons.length; i++) { - moduleConfigs.put(swerveDriveJson.modules[i], i); - File moduleFile = new File(directory, "modules/" + swerveDriveJson.modules[i]); - assert moduleFile.exists(); - moduleJsons[i] = new ObjectMapper().readValue(moduleFile, ModuleJson.class); - } - } - - /** - * Get the swerve module by the json name. - * - * @param name JSON name. - * @param driveConfiguration {@link SwerveDriveConfiguration} to pull from. - * @return {@link SwerveModuleConfiguration} based on the file. - */ - public static SwerveModule getModuleConfigurationByName( - String name, SwerveDriveConfiguration driveConfiguration) { - return driveConfiguration.modules[moduleConfigs.get(name + ".json")]; - } - - /** - * Check directory structure. - * - * @param directory JSON Configuration Directory - */ - private void checkDirectory(File directory) { - assert new File(directory, "swervedrive.json").exists(); - assert new File(directory, "controllerproperties.json").exists(); - assert new File(directory, "modules").exists() && new File(directory, "modules").isDirectory(); - assert new File(directory, "modules/pidfproperties.json").exists(); - assert new File(directory, "modules/physicalproperties.json").exists(); - } - - /** - * Create {@link SwerveDrive} from JSON configuration directory. - * - * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular - * acceleration used in {@link swervelib.SwerveController} and drive feedforward in {@link - * SwerveMath#createDriveFeedforward(double, double, double)}. - * @return {@link SwerveDrive} instance. - */ - public SwerveDrive createSwerveDrive(double maxSpeed) { - return createSwerveDrive( - SwerveMath.createDriveFeedforward( - physicalPropertiesJson.optimalVoltage, - maxSpeed, - physicalPropertiesJson.wheelGripCoefficientOfFriction), - maxSpeed); - } - - /** - * Create {@link SwerveDrive} from JSON configuration directory. - * - * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular - * acceleration used in {@link swervelib.SwerveController} and drive feedforward in {@link - * SwerveMath#createDriveFeedforward(double, double, double)}. - * @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor - * controller PID loop units to degrees, usually created using {@link - * SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. - * @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop - * units to meters per rotation, usually created using {@link - * SwerveMath#calculateMetersPerRotation(double, double, double)}. - * @return {@link SwerveDrive} instance. - */ - public SwerveDrive createSwerveDrive( - double maxSpeed, double angleMotorConversionFactor, double driveMotorConversion) { - physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor; - physicalPropertiesJson.conversionFactor.drive = driveMotorConversion; - return createSwerveDrive( - SwerveMath.createDriveFeedforward( - physicalPropertiesJson.optimalVoltage, - maxSpeed, - physicalPropertiesJson.wheelGripCoefficientOfFriction), - maxSpeed); - } - - /** - * Create {@link SwerveDrive} from JSON configuration directory. - * - * @param driveFeedforward Drive feedforward to use for swerve modules, should be created using - * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. - * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration - * in {@link swervelib.SwerveController} of the robot. - * @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor - * controller PID loop units to degrees, usually created using {@link - * SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. - * @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop - * units to meters per rotation, usually created using {@link - * SwerveMath#calculateMetersPerRotation(double, double, double)}. - * @return {@link SwerveDrive} instance. - */ - public SwerveDrive createSwerveDrive( - SimpleMotorFeedforward driveFeedforward, - double maxSpeed, - double angleMotorConversionFactor, - double driveMotorConversion) { - physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor; - physicalPropertiesJson.conversionFactor.drive = driveMotorConversion; - return createSwerveDrive(driveFeedforward, maxSpeed); - } - - /** - * Create {@link SwerveDrive} from JSON configuration directory. - * - * @param driveFeedforward Drive feedforward to use for swerve modules, should be created using - * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. - * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration - * in {@link swervelib.SwerveController} of the robot - * @return {@link SwerveDrive} instance. - */ - public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed) { - SwerveModuleConfiguration[] moduleConfigurations = - new SwerveModuleConfiguration[moduleJsons.length]; - for (int i = 0; i < moduleConfigurations.length; i++) { - ModuleJson module = moduleJsons[i]; - moduleConfigurations[i] = - module.createModuleConfiguration( - pidfPropertiesJson.angle, - pidfPropertiesJson.drive, - physicalPropertiesJson.createPhysicalProperties(), - swerveDriveJson.modules[i]); - } - SwerveDriveConfiguration swerveDriveConfiguration = - new SwerveDriveConfiguration( - moduleConfigurations, - swerveDriveJson.imu.createIMU(), - swerveDriveJson.invertedIMU, - driveFeedforward, - physicalPropertiesJson.createPhysicalProperties()); - - return new SwerveDrive( - swerveDriveConfiguration, - controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), - maxSpeed); - } -} diff --git a/src/main/java/frc/robot/util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java b/src/main/java/frc/robot/util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java deleted file mode 100644 index 34503da7..00000000 --- a/src/main/java/frc/robot/util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java +++ /dev/null @@ -1,467 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2024 FRC 3481 -// https://github.com/BroncBotz3481/YAGSL -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.util.YAGSL_AzRBSI; - -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfigurator; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import swervelib.encoders.SwerveAbsoluteEncoder; -import swervelib.motors.SwerveMotor; -import swervelib.parser.PIDFConfig; -import swervelib.telemetry.SwerveDriveTelemetry; - -/** {@link com.ctre.phoenix6.hardware.TalonFX} Swerve Motor. Made by Team 1466 WebbRobotics. */ -public class TalonFXSwerve_RBSI extends SwerveMotor { - - /** Wait time for status frames to show up. */ - public static double STATUS_TIMEOUT_SECONDS = 0.02; - - /** Factory default already occurred. */ - private final boolean factoryDefaultOccurred = false; - - /** Whether the absolute encoder is integrated. */ - private final boolean absoluteEncoder = false; - - /** Motion magic angle voltage setter. */ - private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); - - /** Velocity voltage setter for controlling drive motor. */ - private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); - - /** TalonFX motor controller. */ - TalonFX motor; - - /** Conversion factor for the motor. */ - private double conversionFactor; - - /** Current TalonFX configuration. */ - private TalonFXConfiguration configuration = new TalonFXConfiguration(); - - /** Current TalonFX Configurator. */ - private TalonFXConfigurator cfg; - - /** - * Constructor for TalonFX swerve motor. - * - * @param motor Motor to use. - * @param isDriveMotor Whether this motor is a drive motor. - */ - public TalonFXSwerve_RBSI(TalonFX motor, boolean isDriveMotor) { - this.isDriveMotor = isDriveMotor; - this.motor = motor; - this.cfg = motor.getConfigurator(); - - factoryDefaults(); - clearStickyFaults(); - - // if (SwerveDriveTelemetry.isSimulation) - // { - //// PhysicsSim.getInstance().addTalonFX(motor, .25, 6800); - // } - } - - /** - * Construct the TalonFX swerve motor given the ID and CANBus. - * - * @param id ID of the TalonFX on the CANBus. - * @param canbus CANBus on which the TalonFX is on. - * @param isDriveMotor Whether the motor is a drive or steering motor. - */ - public TalonFXSwerve_RBSI(int id, String canbus, boolean isDriveMotor) { - this(new TalonFX(id, canbus), isDriveMotor); - } - - /** - * Construct the TalonFX swerve motor given the ID. - * - * @param id ID of the TalonFX on the RIO canbus. - * @param isDriveMotor Whether the motor is a drive or steering motor. - */ - public TalonFXSwerve_RBSI(int id, boolean isDriveMotor) { - this(new TalonFX(id), isDriveMotor); - } - - /** Configure the factory defaults. */ - @Override - public void factoryDefaults() { - if (!factoryDefaultOccurred) { - configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; - configuration.ClosedLoopGeneral.ContinuousWrap = true; - cfg.apply(configuration); - - m_angleVoltageSetter.UpdateFreqHz = 0; - // m_angleVoltageExpoSetter.UpdateFreqHz = 0; - m_velocityVoltageSetter.UpdateFreqHz = 0; - // motor.configFactoryDefault(); - // motor.setSensorPhase(true); - // motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); - // motor.configNeutralDeadband(0.001); - } - } - - /** Clear the sticky faults on the motor controller. */ - @Override - public void clearStickyFaults() { - motor.clearStickyFaults(); - } - - /** - * Set the absolute encoder to be a compatible absolute encoder. - * - * @param encoder The encoder to use. - */ - @Override - public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { - - // if (encoder == null) - // { - // absoluteEncoder = null; - // configureSparkMax(() -> pid.setFeedbackDevice(this.encoder)); - // velocity = this.encoder::getVelocity; - // position = this.encoder::getPosition; - // } else if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) - // { - // DriverStation.reportWarning( - // "IF possible configure the encoder offset in the REV Hardware Client instead of using - // the" + - // " absoluteEncoderOffset in the Swerve Module JSON!", - // false); - // absoluteEncoder = encoder; - // configureSparkMax(() -> pid.setFeedbackDevice((MotorFeedbackSensor) - // absoluteEncoder.getAbsoluteEncoder())); - // velocity = absoluteEncoder::getVelocity; - // position = absoluteEncoder::getAbsolutePosition; - - // } - - return this; - } - - /** - * Configure the integrated encoder for the swerve module. Sets the conversion factors for - * position and velocity. - * - * @param positionConversionFactor The conversion factor to apply for position. - *


- * Degrees:
- * - * 360 / (angleGearRatio * encoderTicksPerRotation) - *
- *


- * Meters:
- * - * (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation) - * - */ - @Override - public void configureIntegratedEncoder(double positionConversionFactor) { - cfg.refresh(configuration); - - positionConversionFactor = 1 / positionConversionFactor; - if (!isDriveMotor) { - positionConversionFactor *= 360; - } - conversionFactor = positionConversionFactor; - - configuration.MotionMagic = - configuration.MotionMagic.withMotionMagicCruiseVelocity(100.0 / positionConversionFactor) - .withMotionMagicAcceleration((100.0 / positionConversionFactor) / 0.100) - .withMotionMagicExpo_kV(0.12 * positionConversionFactor) - .withMotionMagicExpo_kA(0.1); - - configuration.Feedback.withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor) - .withSensorToMechanismRatio(positionConversionFactor); - - cfg.apply(configuration); - // Taken from democat's library. - // https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16 - configureCANStatusFrames(250); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information - */ - public void configureCANStatusFrames(int CANStatus1) { - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information - * @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed - * Supply Current Measurement, Sticky Fault Information - * @param CANStatus3 Quadrature Information - * @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature - * @param CANStatus8 Pulse Width Information - * @param CANStatus10 Motion Profiling/Motion Magic Information - * @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1) - * @param CANStatus13 PID0 (Primary PID) Information - * @param CANStatus14 PID1 (Auxiliary PID) Information - * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX) - * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current - * Measurement - */ - public void configureCANStatusFrames( - int CANStatus1, - int CANStatus2, - int CANStatus3, - int CANStatus4, - int CANStatus8, - int CANStatus10, - int CANStatus12, - int CANStatus13, - int CANStatus14, - int CANStatus21, - int CANStatusCurrent) { - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, - // CANStatusCurrent); - - // TODO: Configure Status Frame 2 thru 21 if necessary - // https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods - } - - /** - * Configure the PIDF values for the closed loop controller. 0 is disabled or off. - * - * @param config Configuration class holding the PIDF values. - */ - @Override - public void configurePIDF(PIDFConfig config) { - - cfg.refresh(configuration.Slot0); - cfg.apply( - configuration.Slot0.withKP(config.p).withKI(config.i).withKD(config.d).withKS(config.f)); - // configuration.slot0.integralZone = config.iz; - // configuration.slot0.closedLoopPeakOutput = config.output.max; - } - - /** - * Configure the PID wrapping for the position closed loop controller. - * - * @param minInput Minimum PID input. - * @param maxInput Maximum PID input. - */ - @Override - public void configurePIDWrapping(double minInput, double maxInput) { - cfg.refresh(configuration.ClosedLoopGeneral); - configuration.ClosedLoopGeneral.ContinuousWrap = true; - cfg.apply(configuration.ClosedLoopGeneral); - } - - /** - * Set the idle mode. - * - * @param isBrakeMode Set the brake mode. - */ - @Override - public void setMotorBrake(boolean isBrakeMode) { - motor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast); - } - - /** - * Set the motor to be inverted. - * - * @param inverted State of inversion. - */ - @Override - public void setInverted(boolean inverted) { - // Timer.delay(1); - motor.setInverted(inverted); - } - - /** Save the configurations from flash to EEPROM. */ - @Override - public void burnFlash() { - // Do nothing - } - - /** - * Set the percentage output. - * - * @param percentOutput percent out for the motor controller. - */ - @Override - public void set(double percentOutput) { - motor.set(percentOutput); - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in MPS or Angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - */ - @Override - public void setReference(double setpoint, double feedforward) { - setReference(setpoint, feedforward, getPosition()); - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in meters per second or angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - * @param position Only used on the angle motor, the position of the motor in degrees. - */ - @Override - public void setReference(double setpoint, double feedforward, double position) { - // if (SwerveDriveTelemetry.isSimulation) - // { - // PhysicsSim.getInstance().run(); - // } - - if (isDriveMotor) { - motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint).withFeedForward(feedforward)); - } else { - motor.setControl(m_angleVoltageSetter.withPosition(setpoint / 360.0)); - } - } - - /** - * Get the voltage output of the motor controller. - * - * @return Voltage output. - */ - @Override - public double getVoltage() { - return motor.getMotorVoltage().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(); - } - - /** - * Set the voltage of the motor. - * - * @param voltage Voltage to set. - */ - @Override - public void setVoltage(double voltage) { - motor.setVoltage(voltage); - } - - /** - * Get the applied dutycycle output. - * - * @return Applied dutycycle output to the motor. - */ - @Override - public double getAppliedOutput() { - return motor.getDutyCycle().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(); - } - - /** - * Get the velocity of the integrated encoder. - * - * @return velocity in Meters Per Second, or Degrees per Second. - */ - @Override - public double getVelocity() { - return motor.getVelocity().getValue(); - } - - /** - * Get the position of the integrated encoder. - * - * @return Position in Meters or Degrees. - */ - @Override - public double getPosition() { - return motor.getPosition().getValue(); - } - - /** - * Set the integrated encoder position. - * - * @param position Integrated encoder position. Should be angle in degrees or meters. - */ - @Override - public void setPosition(double position) { - if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) { - position = position < 0 ? (position % 360) + 360 : position; - cfg.setPosition(position / 360); - } - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param nominalVoltage Nominal voltage for operation to output to. - */ - @Override - public void setVoltageCompensation(double nominalVoltage) { - // Do not implement - } - - /** - * Set the current limit for the swerve drive motor, remember this may cause jumping if used in - * conjunction with voltage compensation. This is useful to protect the motor from current spikes. - * - * @param currentLimit Current limit in AMPS at free speed. - */ - @Override - public void setCurrentLimit(int currentLimit) { - cfg.refresh(configuration.CurrentLimits); - cfg.apply( - configuration.CurrentLimits.withStatorCurrentLimit(currentLimit) - .withStatorCurrentLimitEnable(true)); - } - - /** - * Set the maximum rate the open/closed loop output can change by. - * - * @param rampRate Time in seconds to go from 0 to full throttle. - */ - @Override - public void setLoopRampRate(double rampRate) { - cfg.refresh(configuration.ClosedLoopRamps); - cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate)); - } - - /** - * Get the motor object from the module. - * - * @return Motor object. - */ - @Override - public Object getMotor() { - return motor; - } - - /** - * Queries whether the absolute encoder is directly attached to the motor controller. - * - * @return connected absolute encoder state. - */ - @Override - public boolean isAttachedAbsoluteEncoder() { - return absoluteEncoder; - } -} From bf463c26c309f8076bd7aff419b2deb332fb7cb8 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 23 Oct 2024 16:49:20 -0700 Subject: [PATCH 32/57] Drive subsystem == Phoenix6/CTRE modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/generated/TunerConstants.java modified: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java deleted: src/main/java/frc/robot/subsystems/swervedrive/underlying/Phoenix6Swerve.java modified: src/main/java/frc/robot/subsystems/vision/Vision.java new file: src/test/CurrentLimitTests.java new file: src/test/FusedCANcoderTests.java new file: src/test/LatencyCompensationTests.java --- src/main/java/frc/robot/RobotContainer.java | 74 +----- .../frc/robot/generated/TunerConstants.java | 6 +- .../swervedrive/SwerveSubsystem.java | 248 ++++++++++++++++-- .../underlying/Phoenix6Swerve.java | 214 --------------- .../frc/robot/subsystems/vision/Vision.java | 6 +- src/test/CurrentLimitTests.java | 167 ++++++++++++ src/test/FusedCANcoderTests.java | 185 +++++++++++++ src/test/LatencyCompensationTests.java | 99 +++++++ 8 files changed, 696 insertions(+), 303 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/swervedrive/underlying/Phoenix6Swerve.java create mode 100644 src/test/CurrentLimitTests.java create mode 100644 src/test/FusedCANcoderTests.java create mode 100644 src/test/LatencyCompensationTests.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f479002c..4890886f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,19 +19,16 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.AprilTagConstants; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; -import frc.robot.Constants.OperatorConstants; -import frc.robot.commands.swervedrive.drivebase.AbsoluteDriveAdv; +import frc.robot.generated.TunerConstants; import frc.robot.subsystems.flywheel_example.Flywheel; import frc.robot.subsystems.flywheel_example.FlywheelIO; import frc.robot.subsystems.flywheel_example.FlywheelIOSim; @@ -76,7 +73,7 @@ public RobotContainer() { case REAL: // Real robot, instantiate hardware IO implementations // YAGSL drivebase, get config from deploy directory - m_drivebase = new SwerveSubsystem(Constants.getSwerve()); + m_drivebase = TunerConstants.DriveTrain; m_flywheel = new Flywheel(new FlywheelIOSim()); // new Flywheel(new FlywheelIOTalonFX()); m_vision = new Vision( @@ -86,14 +83,14 @@ public RobotContainer() { case SIM: // Sim robot, instantiate physics sim IO implementations - m_drivebase = new SwerveSubsystem(Constants.getSwerve()); + m_drivebase = TunerConstants.DriveTrain; m_flywheel = new Flywheel(new FlywheelIOSim()); m_vision = new Vision(this::getAprilTagLayoutType); break; default: // Replayed robot, disable IO implementations - m_drivebase = new SwerveSubsystem(Constants.getSwerve()); + m_drivebase = TunerConstants.DriveTrain; m_flywheel = new Flywheel(new FlywheelIO() {}); m_vision = new Vision(this::getAprilTagLayoutType, new VisionIO() {}, new VisionIO() {}); break; @@ -110,67 +107,12 @@ public RobotContainer() { } /** Use this method to define your TeleOp commands. */ - private void defineTeleopCommands() { - - // Applies deadbands and inverts controls because joysticks - // are back-right positive while robot - // controls are front-left positive - // left stick controls translation - // right stick controls the rotational velocity - // buttons are quick rotation positions to different ways to face - // WARNING: default buttons are on the same buttons as the ones defined in configureBindings - AbsoluteDriveAdv closedAbsoluteDriveAdv = - new AbsoluteDriveAdv( - m_drivebase, - () -> -MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), - () -> -MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> - -MathUtil.applyDeadband(driverXbox.getRightX(), OperatorConstants.RIGHT_X_DEADBAND), - driverXbox.getHID()::getYButtonPressed, - driverXbox.getHID()::getAButtonPressed, - driverXbox.getHID()::getXButtonPressed, - driverXbox.getHID()::getBButtonPressed); - - // Applies deadbands and inverts controls because joysticks - // are back-right positive while robot - // controls are front-left positive - // left stick controls translation - // right stick controls the desired angle NOT angular rotation - Command driveFieldOrientedDirectAngle = - m_drivebase.driveCommand( - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> driverXbox.getRightX(), - () -> driverXbox.getRightY()); - - // Applies deadbands and inverts controls because joysticks - // are back-right positive while robot - // controls are front-left positive - // left stick controls translation - // right stick controls the angular velocity of the robot - Command driveFieldOrientedAnglularVelocity = - m_drivebase.driveCommand( - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> driverXbox.getRightX() * 0.5); - - Command driveFieldOrientedDirectAngleSim = - m_drivebase.simDriveCommand( - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> driverXbox.getRawAxis(2)); - - m_drivebase.setDefaultCommand( - !RobotBase.isSimulation() - ? driveFieldOrientedDirectAngle - : driveFieldOrientedDirectAngleSim); - } + private void defineTeleopCommands() {} /** Use this method to define your Autonomous commands for use with PathPlanner / Choreo */ private void defineAutoCommands() { - NamedCommands.registerCommand( - "Zero", Commands.runOnce(() -> m_drivebase.zeroGyroWithAlliance())); + NamedCommands.registerCommand("Zero", Commands.runOnce(() -> m_drivebase.tareEverything())); } /** @@ -185,7 +127,7 @@ private void defineAutoCommands() { private void configureBindings() { // Manually Re-Zero the Gyro - driverXbox.y().onTrue(Commands.runOnce(() -> m_drivebase.zeroGyroWithAlliance())); + driverXbox.y().onTrue(Commands.runOnce(() -> m_drivebase.tareEverything())); // Example button bindings from YAGSL // if (DriverStation.isTest()) @@ -239,8 +181,8 @@ public void setDriveMode() { configureBindings(); } + /** Set the motor neutral mode to BRAKE / COAST for T/F */ public void setMotorBrake(boolean brake) { - m_drivebase.setMotorBrake(brake); } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 0520456b..e2bb17bb 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -11,7 +11,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.math.util.Units; -import frc.robot.subsystems.swervedrive.underlying.Phoenix6Swerve; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html @@ -182,6 +182,6 @@ public class TunerConstants { Units.inchesToMeters(kBackRightYPosInches), kInvertRightSide); - public static final Phoenix6Swerve DriveTrain = - new Phoenix6Swerve(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); + public static final SwerveSubsystem DriveTrain = + new SwerveSubsystem(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 3d2f6449..51d9e764 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -14,42 +14,252 @@ // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +// +// NOTE: This module based on the CTRE Phoenix6 examples +// https://github.com/CrossTheRoadElec/Phoenix6-Examples package frc.robot.subsystems.swervedrive; -import edu.wpi.first.wpilibj.Filesystem; -import frc.robot.Constants.SwerveType; +import static edu.wpi.first.units.Units.Volts; +import static frc.robot.Constants.DrivebaseConstants.*; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.swervedrive.underlying.Phoenix6Swerve; -import frc.robot.subsystems.swervedrive.underlying.YAGSLSwerve; -import java.io.File; +import java.util.function.Supplier; /** - * Az-RBSI swerve class that generalizes the swerve subsystem to allow for the use of either - * Phoenix6 or YAGSL underlying libraries + * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used + * in command-based projects easily. */ -public class SwerveSubsystem extends Phoenix6Swerve { +public class SwerveSubsystem extends SwerveDrivetrain implements Subsystem { + private static final double kSimLoopPeriod = 0.005; // 5 ms + private Notifier m_simNotifier = null; + private double m_lastSimTime; - private YAGSLSwerve y_swerve; + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private final Rotation2d RedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean hasAppliedOperatorPerspective = false; - public SwerveSubsystem(Enum swerveType) { + private final SwerveRequest.ApplyChassisSpeeds AutoRequest = + new SwerveRequest.ApplyChassisSpeeds(); - if (swerveType == SwerveType.YAGSL) { + private final SwerveRequest.SysIdSwerveTranslation TranslationCharacterization = + new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveRotation RotationCharacterization = + new SwerveRequest.SysIdSwerveRotation(); + private final SwerveRequest.SysIdSwerveSteerGains SteerCharacterization = + new SwerveRequest.SysIdSwerveSteerGains(); - // Load up the YASGL things into this class - y_swerve = new YAGSLSwerve(new File(Filesystem.getDeployDirectory(), "swerve")); + /* Use one of these sysidroutines for your particular test */ + private SysIdRoutine SysIdRoutineTranslation = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + Volts.of(4), + null, + (state) -> SignalLogger.writeString("state", state.toString())), + new SysIdRoutine.Mechanism( + (volts) -> setControl(TranslationCharacterization.withVolts(volts)), null, this)); - } else if (swerveType == SwerveType.PHOENIX6) { + private final SysIdRoutine SysIdRoutineRotation = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + Volts.of(4), + null, + (state) -> SignalLogger.writeString("state", state.toString())), + new SysIdRoutine.Mechanism( + (volts) -> setControl(RotationCharacterization.withVolts(volts)), null, this)); + private final SysIdRoutine SysIdRoutineSteer = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + Volts.of(7), + null, + (state) -> SignalLogger.writeString("state", state.toString())), + new SysIdRoutine.Mechanism( + (volts) -> setControl(SteerCharacterization.withVolts(volts)), null, this)); - // Load up the Phoenix6 things into this class - p_swerve = TunerConstants.DriveTrain; + /* Change this to the sysid routine you want to test */ + private final SysIdRoutine RoutineToApply = SysIdRoutineTranslation; - } else { + public SwerveSubsystem( + SwerveDrivetrainConstants driveTrainConstants, + double OdometryUpdateFrequency, + SwerveModuleConstants... modules) { + super(driveTrainConstants, OdometryUpdateFrequency, modules); + configurePathPlanner(); + if (Utils.isSimulation()) { + startSimThread(); + } + } - // Raise an error because something unknown was passed + public SwerveSubsystem( + SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { + super(driveTrainConstants, modules); + configurePathPlanner(); + if (Utils.isSimulation()) { + startSimThread(); + } + } + private void configurePathPlanner() { + double driveBaseRadius = 0; + for (var moduleLocation : m_moduleLocations) { + driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm()); } + + AutoBuilder.configureHolonomic( + () -> this.getState().Pose, // Supplier of current robot pose + this::seedFieldRelative, // Consumer for seeding pose against auto + this::getCurrentRobotChassisSpeeds, + (speeds) -> + this.setControl( + AutoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot + new HolonomicPathFollowerConfig( + new PIDConstants(10, 0, 0), + new PIDConstants(10, 0, 0), + TunerConstants.kSpeedAt12VoltsMps, + driveBaseRadius, + new ReplanningConfig()), + () -> + DriverStation.getAlliance().orElse(Alliance.Blue) + == Alliance + .Red, // Assume the path needs to be flipped for Red vs Blue, this is normally + // the case + this); // Subsystem for requirements } - // + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } + + public Command getAutoPath(String pathName) { + return new PathPlannerAuto(pathName); + } + + /* + * Both the sysid commands are specific to one particular sysid routine, change + * which one you're trying to characterize + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return RoutineToApply.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return RoutineToApply.dynamic(direction); + } + + public ChassisSpeeds getCurrentRobotChassisSpeeds() { + return m_kinematics.toChassisSpeeds(getState().ModuleStates); + } + + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = + new Notifier( + () -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } + + @Override + public void periodic() { + /* Periodically try to apply the operator perspective */ + /* If we haven't applied the operator perspective before, then we should apply it regardless of DS state */ + /* This allows us to correct the perspective in case the robot code restarts mid-match */ + /* Otherwise, only check and apply the operator perspective if the DS is disabled */ + /* This ensures driving behavior doesn't change until an explicit disable event occurs during testing*/ + if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance() + .ifPresent( + (allianceColor) -> { + this.setOperatorPerspectiveForward( + allianceColor == Alliance.Red + ? RedAlliancePerspectiveRotation + : BlueAlliancePerspectiveRotation); + hasAppliedOperatorPerspective = true; + }); + } + } + + /** + * Get the path follower with events. + * + * @param pathName PathPlanner path name. + * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. + */ + public Command getAutonomousCommand(String pathName) { + // Create a path following command using AutoBuilder. This will also trigger event markers. + return new PathPlannerAuto(pathName); + } + + /** + * Use PathPlanner Path finding to go to a point on the field. + * + * @param pose Target {@link Pose2d} to go to. + * @return PathFinding command + */ + public Command driveToPose(Pose2d pose) { + // Create the constraints to use while pathfinding + PathConstraints constraints = + new PathConstraints( + MAX_LINEAR_SPEED, MAX_LINEAR_ACCEL, MAX_ANGULAR_SPEED, MAX_ANGULAR_ACCEL); + + // Since AutoBuilder is configured, we can use it to build pathfinding commands + return AutoBuilder.pathfindToPose( + pose, + constraints, + 0.0, // Goal end velocity in meters/sec + 0.0 // Rotation delay distance in meters. This is how far the robot should travel before + // attempting to rotate. + ); + } + + /** + * Sets the drive motors to brake/coast mode. + * + * @param brake True to set motors to brake mode, false for coast. + */ + public void setMotorBrake(boolean brake) { + if (brake) { + this.configNeutralMode(NeutralModeValue.Brake); + } else { + this.configNeutralMode(NeutralModeValue.Coast); + } + } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/underlying/Phoenix6Swerve.java b/src/main/java/frc/robot/subsystems/swervedrive/underlying/Phoenix6Swerve.java deleted file mode 100644 index 95e22f64..00000000 --- a/src/main/java/frc/robot/subsystems/swervedrive/underlying/Phoenix6Swerve.java +++ /dev/null @@ -1,214 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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. -// -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. -// -// NOTE: This module based on the CTRE Phoenix6 examples -// https://github.com/CrossTheRoadElec/Phoenix6-Examples - -package frc.robot.subsystems.swervedrive.underlying; - -import static edu.wpi.first.units.Units.Volts; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; -import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; -import com.pathplanner.lib.util.ReplanningConfig; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.generated.TunerConstants; -import java.util.function.Supplier; - -/** - * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used - * in command-based projects easily. - */ -public class Phoenix6Swerve extends SwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private final Rotation2d RedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean hasAppliedOperatorPerspective = false; - - private final SwerveRequest.ApplyChassisSpeeds AutoRequest = - new SwerveRequest.ApplyChassisSpeeds(); - - private final SwerveRequest.SysIdSwerveTranslation TranslationCharacterization = - new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveRotation RotationCharacterization = - new SwerveRequest.SysIdSwerveRotation(); - private final SwerveRequest.SysIdSwerveSteerGains SteerCharacterization = - new SwerveRequest.SysIdSwerveSteerGains(); - - /* Use one of these sysidroutines for your particular test */ - private SysIdRoutine SysIdRoutineTranslation = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - Volts.of(4), - null, - (state) -> SignalLogger.writeString("state", state.toString())), - new SysIdRoutine.Mechanism( - (volts) -> setControl(TranslationCharacterization.withVolts(volts)), null, this)); - - private final SysIdRoutine SysIdRoutineRotation = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - Volts.of(4), - null, - (state) -> SignalLogger.writeString("state", state.toString())), - new SysIdRoutine.Mechanism( - (volts) -> setControl(RotationCharacterization.withVolts(volts)), null, this)); - private final SysIdRoutine SysIdRoutineSteer = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - Volts.of(7), - null, - (state) -> SignalLogger.writeString("state", state.toString())), - new SysIdRoutine.Mechanism( - (volts) -> setControl(SteerCharacterization.withVolts(volts)), null, this)); - - /* Change this to the sysid routine you want to test */ - private final SysIdRoutine RoutineToApply = SysIdRoutineTranslation; - - public Phoenix6Swerve( - SwerveDrivetrainConstants driveTrainConstants, - double OdometryUpdateFrequency, - SwerveModuleConstants... modules) { - super(driveTrainConstants, OdometryUpdateFrequency, modules); - configurePathPlanner(); - if (Utils.isSimulation()) { - startSimThread(); - } - } - - public Phoenix6Swerve( - SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { - super(driveTrainConstants, modules); - configurePathPlanner(); - if (Utils.isSimulation()) { - startSimThread(); - } - } - - private void configurePathPlanner() { - double driveBaseRadius = 0; - for (var moduleLocation : m_moduleLocations) { - driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm()); - } - - AutoBuilder.configureHolonomic( - () -> this.getState().Pose, // Supplier of current robot pose - this::seedFieldRelative, // Consumer for seeding pose against auto - this::getCurrentRobotChassisSpeeds, - (speeds) -> - this.setControl( - AutoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot - new HolonomicPathFollowerConfig( - new PIDConstants(10, 0, 0), - new PIDConstants(10, 0, 0), - TunerConstants.kSpeedAt12VoltsMps, - driveBaseRadius, - new ReplanningConfig()), - () -> - DriverStation.getAlliance().orElse(Alliance.Blue) - == Alliance - .Red, // Assume the path needs to be flipped for Red vs Blue, this is normally - // the case - this); // Subsystem for requirements - } - - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } - - public Command getAutoPath(String pathName) { - return new PathPlannerAuto(pathName); - } - - /* - * Both the sysid commands are specific to one particular sysid routine, change - * which one you're trying to characterize - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return RoutineToApply.quasistatic(direction); - } - - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return RoutineToApply.dynamic(direction); - } - - public ChassisSpeeds getCurrentRobotChassisSpeeds() { - return m_kinematics.toChassisSpeeds(getState().ModuleStates); - } - - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); - - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = - new Notifier( - () -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; - - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); - } - - @Override - public void periodic() { - /* Periodically try to apply the operator perspective */ - /* If we haven't applied the operator perspective before, then we should apply it regardless of DS state */ - /* This allows us to correct the perspective in case the robot code restarts mid-match */ - /* Otherwise, only check and apply the operator perspective if the DS is disabled */ - /* This ensures driving behavior doesn't change until an explicit disable event occurs during testing*/ - if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance() - .ifPresent( - (allianceColor) -> { - this.setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? RedAlliancePerspectiveRotation - : BlueAlliancePerspectiveRotation); - hasAppliedOperatorPerspective = true; - }); - } - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 2b922e07..e2f8e564 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -40,6 +40,7 @@ import frc.robot.Robot; import frc.robot.RobotContainer; import frc.robot.subsystems.vision.VisionIO.VisionIOInputs; +import frc.robot.util.VirtualSubsystem; import java.awt.Desktop; import java.util.ArrayList; import java.util.HashMap; @@ -66,7 +67,7 @@ * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from * https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads */ -public class Vision { +public class Vision extends VirtualSubsystem { /** Photon Vision Simulation */ public VisionSystemSim visionSim; @@ -132,6 +133,9 @@ public Vision(Supplier aprilTagTypeSupplier, VisionIO... io) } } + @Override + public void periodic() {} + /** * Calculates a target pose relative to an AprilTag on the field. * diff --git a/src/test/CurrentLimitTests.java b/src/test/CurrentLimitTests.java new file mode 100644 index 00000000..dcf25f9c --- /dev/null +++ b/src/test/CurrentLimitTests.java @@ -0,0 +1,167 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright (c) 2024 Cross The Road Electronics +// https://github.com/CrossTheRoadElec/Phoenix6-Examples +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +import static org.junit.jupiter.api.Assertions.assertTrue; + +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import java.util.function.Supplier; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +public class CurrentLimitTests implements AutoCloseable { + final int CONFIG_RETRY_COUNT = 5; + + TalonFX talon; + + @Override + public void close() { + /* destroy our TalonFX object */ + talon.close(); + } + + @BeforeEach + public void constructDevices() { + assert HAL.initialize(500, 0); + + talon = new TalonFX(0); + + /* enable the robot */ + DriverStationSim.setEnabled(true); + DriverStationSim.notifyNewData(); + + /* delay ~100ms so the devices can start up and enable */ + Timer.delay(0.100); + } + + @AfterEach + void shutdown() { + close(); + } + + @Test + public void robotIsEnabled() { + /* verify that the robot is enabled */ + assertTrue(DriverStation.isEnabled()); + } + + @Test + public void testStatorLimit() { + var statorCurrent = talon.getStatorCurrent(); + + /* Configure a stator limit of 20 amps */ + TalonFXConfiguration toConfigure = new TalonFXConfiguration(); + CurrentLimitsConfigs currentLimitConfigs = toConfigure.CurrentLimits; + currentLimitConfigs.StatorCurrentLimit = 20; + currentLimitConfigs.StatorCurrentLimitEnable = false; // Start with stator limits off + + retryConfigApply(() -> talon.getConfigurator().apply(toConfigure)); + + /* Put the talon in a stall, which should produce a lot of current */ + talon.setControl(new DutyCycleOut(1)); + /* wait for the control to apply */ + Timer.delay(0.020); + + /* Get the next update for stator current */ + statorCurrent.waitForUpdate(1); + + System.out.println("Stator current is " + statorCurrent); + assertTrue(statorCurrent.getValue() > 100); // Stator current should be in excess of 100 amps + + /* Now apply the stator current limit */ + currentLimitConfigs.StatorCurrentLimitEnable = true; + retryConfigApply(() -> talon.getConfigurator().apply(currentLimitConfigs)); + + /* wait for the current to drop */ + Timer.delay(0.500); + + /* Get the next update for stator current */ + statorCurrent.waitForUpdate(1); + + System.out.println("Stator current is " + statorCurrent); + assertTrue(statorCurrent.getValue() < 25); // Give some wiggle room + } + + @Test + public void testSupplyLimit() { + var supplyCurrent = talon.getSupplyCurrent(); + + /* Configure a supply limit of 20 amps */ + TalonFXConfiguration toConfigure = new TalonFXConfiguration(); + CurrentLimitsConfigs currentLimitConfigs = toConfigure.CurrentLimits; + currentLimitConfigs.SupplyCurrentLimit = 5; + currentLimitConfigs.SupplyCurrentThreshold = 10; + currentLimitConfigs.SupplyTimeThreshold = 1.0; + currentLimitConfigs.StatorCurrentLimitEnable = false; // Start with supply limits off + + retryConfigApply(() -> talon.getConfigurator().apply(toConfigure)); + + /* Put the talon in a stall, which should produce a lot of current */ + talon.setControl(new DutyCycleOut(1)); + /* wait for the control to apply */ + Timer.delay(0.100); + + /* Get the next update for supply current */ + supplyCurrent.waitForUpdate(1); + + System.out.println("Supply current is " + supplyCurrent); + assertTrue(supplyCurrent.getValue() > 80); // Supply current should be in excess of 80 amps + + /* Now apply the supply current limit */ + currentLimitConfigs.SupplyCurrentLimitEnable = true; + retryConfigApply(() -> talon.getConfigurator().apply(currentLimitConfigs)); + + /* wait for the current to adjust */ + Timer.delay(0.500); + + /* Get the next update for supply current */ + supplyCurrent.waitForUpdate(1); + + System.out.println("Supply current is " + supplyCurrent); + assertTrue( + supplyCurrent.getValue() + > 80); // Make sure it's still over 80 amps (time hasn't exceeded 1 second in total) + + /* Wait a full extra couple seconds so the limit kicks in and starts limiting us */ + Timer.delay(2); + + /* Get the next update for supply current */ + supplyCurrent.waitForUpdate(1); + + System.out.println("Supply current is " + supplyCurrent); + assertTrue(supplyCurrent.getValue() < 10); // Give some wiggle room + } + + private void retryConfigApply(Supplier toApply) { + StatusCode finalCode = StatusCode.StatusCodeNotInitialized; + int triesLeftOver = CONFIG_RETRY_COUNT; + do { + finalCode = toApply.get(); + } while (!finalCode.isOK() && --triesLeftOver > 0); + assert (finalCode.isOK()); + } +} diff --git a/src/test/FusedCANcoderTests.java b/src/test/FusedCANcoderTests.java new file mode 100644 index 00000000..077eb269 --- /dev/null +++ b/src/test/FusedCANcoderTests.java @@ -0,0 +1,185 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright (c) 2024 Cross The Road Electronics +// https://github.com/CrossTheRoadElec/Phoenix6-Examples +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import edu.wpi.first.hal.HAL; +import java.util.function.Supplier; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +public class FusedCANcoderTests { + final double SET_DELTA = 0.1; + final int CONFIG_RETRY_COUNT = 5; + + TalonFX talon; + CANcoder cancoder; + + @BeforeEach + public void constructDevices() { + assert HAL.initialize(500, 0); + + talon = new TalonFX(0); + cancoder = new CANcoder(0); + } + + @Test + public void testIndividualPos() { + final double TALON_POSITION = 0.4; + final double CANCODER_POSITION = -3.1; + + /* Factory-default Talon and CANcoder */ + retryConfigApply(() -> talon.getConfigurator().apply(new TalonFXConfiguration())); + retryConfigApply(() -> cancoder.getConfigurator().apply(new CANcoderConfiguration())); + + /* Get sim states */ + var talonSimState = talon.getSimState(); + var cancoderSimState = cancoder.getSimState(); + + /* Wait for signal to update and assert they match the set positions */ + var talonPos = talon.getPosition(); + var cancoderPos = cancoder.getPosition(); + + /* Make sure both are initially set to 0 before messing with sim state */ + retryConfigApply(() -> talonSimState.setRawRotorPosition(0)); + retryConfigApply(() -> cancoderSimState.setRawPosition(0)); + retryConfigApply(() -> talon.setPosition(0)); + retryConfigApply(() -> cancoder.setPosition(0)); + /* Wait for sets to take affect */ + BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos); + + /* Set them to different values */ + retryConfigApply(() -> talonSimState.setRawRotorPosition(TALON_POSITION)); + retryConfigApply(() -> cancoderSimState.setRawPosition(CANCODER_POSITION)); + + BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos); + BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos); + + System.out.println("Talon Pos vs expected: " + talonPos + " vs " + TALON_POSITION); + System.out.println("CANcoder Pos vs expected: " + cancoderPos + " vs " + CANCODER_POSITION); + assertEquals(talonPos.getValue(), TALON_POSITION, SET_DELTA); + assertEquals(cancoderPos.getValue(), CANCODER_POSITION, SET_DELTA); + } + + @Test + public void testFusedCANcoderPos() { + final double TALON_POSITION = 0.4; + final double CANCODER_POSITION = -3.1; + + /* Configure Talon to use Fused CANcoder, factory default CANcoder */ + var configs = new TalonFXConfiguration(); + configs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + configs.Feedback.FeedbackRemoteSensorID = cancoder.getDeviceID(); + retryConfigApply(() -> talon.getConfigurator().apply(configs)); + retryConfigApply(() -> cancoder.getConfigurator().apply(new CANcoderConfiguration())); + + /* Get sim states */ + var talonSimState = talon.getSimState(); + var cancoderSimState = cancoder.getSimState(); + + /* Wait for signal to update and assert they match the set positions */ + var talonPos = talon.getPosition(); + var cancoderPos = cancoder.getPosition(); + + /* Make sure both are initially set to 0 before messing with sim state */ + retryConfigApply(() -> talonSimState.setRawRotorPosition(0)); + retryConfigApply(() -> cancoderSimState.setRawPosition(0)); + retryConfigApply(() -> talon.setPosition(0)); + retryConfigApply(() -> cancoder.setPosition(0)); + /* Wait for sets to take affect */ + BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos); + + /* Set them to different values */ + retryConfigApply(() -> talonSimState.setRawRotorPosition(TALON_POSITION)); + retryConfigApply(() -> cancoderSimState.setRawPosition(CANCODER_POSITION)); + + BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos); + BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos); + + /* Further wait for Talon, since it probably just received the new CANcoder position frame */ + talonPos.waitForUpdate(1.0); + + /* Make sure Talon matches CANcoder, since it should be using CANcoder's position */ + System.out.println("Talon Pos vs expected: " + talonPos + " vs " + CANCODER_POSITION); + System.out.println("CANcoder Pos vs expected: " + cancoderPos + " vs " + CANCODER_POSITION); + assertEquals(talonPos.getValue(), CANCODER_POSITION, SET_DELTA); + assertEquals(cancoderPos.getValue(), CANCODER_POSITION, SET_DELTA); + } + + @Test + public void testRemoteCANcoderPos() { + final double TALON_POSITION = 0.4; + final double CANCODER_POSITION = -3.1; + + /* Configure Talon to use Fused CANcoder, factory default CANcoder */ + var configs = new TalonFXConfiguration(); + configs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + configs.Feedback.FeedbackRemoteSensorID = cancoder.getDeviceID(); + retryConfigApply(() -> talon.getConfigurator().apply(configs)); + retryConfigApply(() -> cancoder.getConfigurator().apply(new CANcoderConfiguration())); + + /* Get sim states */ + var talonSimState = talon.getSimState(); + var cancoderSimState = cancoder.getSimState(); + + /* Wait for signal to update and assert they match the set positions */ + var talonPos = talon.getPosition(); + var cancoderPos = cancoder.getPosition(); + + /* Make sure both are initially set to 0 before messing with sim state */ + retryConfigApply(() -> talonSimState.setRawRotorPosition(0)); + retryConfigApply(() -> cancoderSimState.setRawPosition(0)); + retryConfigApply(() -> talon.setPosition(0)); + retryConfigApply(() -> cancoder.setPosition(0)); + /* Wait for sets to take affect */ + BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos); + + /* Set them to different values */ + retryConfigApply(() -> talonSimState.setRawRotorPosition(TALON_POSITION)); + retryConfigApply(() -> cancoderSimState.setRawPosition(CANCODER_POSITION)); + + BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos); + BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos); + + /* Further wait for Talon, since it probably just received the new CANcoder position frame */ + talonPos.waitForUpdate(1.0); + + /* Make sure Talon matches CANcoder, since it should be using CANcoder's position */ + System.out.println("Talon Pos vs expected: " + talonPos + " vs " + CANCODER_POSITION); + System.out.println("CANcoder Pos vs expected: " + cancoderPos + " vs " + CANCODER_POSITION); + assertEquals(talonPos.getValue(), CANCODER_POSITION, SET_DELTA); + assertEquals(cancoderPos.getValue(), CANCODER_POSITION, SET_DELTA); + } + + private void retryConfigApply(Supplier toApply) { + StatusCode finalCode = StatusCode.StatusCodeNotInitialized; + int triesLeftOver = CONFIG_RETRY_COUNT; + do { + finalCode = toApply.get(); + } while (!finalCode.isOK() && --triesLeftOver > 0); + assert (finalCode.isOK()); + } +} diff --git a/src/test/LatencyCompensationTests.java b/src/test/LatencyCompensationTests.java new file mode 100644 index 00000000..27c696a2 --- /dev/null +++ b/src/test/LatencyCompensationTests.java @@ -0,0 +1,99 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright (c) 2024 Cross The Road Electronics +// https://github.com/CrossTheRoadElec/Phoenix6-Examples +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.hal.HAL; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +public class LatencyCompensationTests { + final double DOUBLE_DELTA = 0.01; + + TalonFX talonfx; + CANcoder cancoder; + + @BeforeEach + public void constructDevices() { + assert HAL.initialize(500, 0); + + talonfx = new TalonFX(0); + cancoder = new CANcoder(0); + } + + @Test + public void testLatencyCompensator() { + final double position = 35; + final double velocity = 24; + /* Initialize by making all the positions 0 */ + talonfx.setPosition(0); + cancoder.setPosition(0); + + /* Set the simulated state of device positions */ + talonfx.getSimState().setRawRotorPosition(position); + talonfx.getSimState().setRotorVelocity(velocity); + cancoder.getSimState().setRawPosition(position); + cancoder.getSimState().setVelocity(velocity); + + /* Perform latency compensation */ + /* Start by getting signals */ + var talonPos = talonfx.getPosition(); + var talonVel = talonfx.getVelocity(); + var cancoderPos = cancoder.getPosition(); + var cancoderVel = cancoder.getVelocity(); + + /* Wait for an update on all of them so they're synchronized */ + StatusCode status = StatusCode.OK; + for (int i = 0; i < 5; ++i) { + System.out.println("Waiting on signals"); + status = BaseStatusSignal.waitForAll(1, talonPos, talonVel, cancoderPos, cancoderVel); + if (status.isOK()) break; + } + assertTrue(status.isOK()); + + /* Wait a bit longer for the latency to actually do some work */ + try { + Thread.sleep(10); + } catch (Exception ex) { + } + /* Calculate how much latency we'd expect */ + double talonLatency = talonPos.getTimestamp().getLatency(); + double compensatedTalonPos = position + (velocity * talonLatency); + double cancoderLatency = cancoderPos.getTimestamp().getLatency(); + double compensatedCANcoderPos = position + (velocity * cancoderLatency); + + /* Calculate compensated values before the assert to avoid timing issue related to it */ + double functionCompensatedTalon = + BaseStatusSignal.getLatencyCompensatedValue(talonPos, talonVel); + double functionCompensatedCANcoder = + BaseStatusSignal.getLatencyCompensatedValue(cancoderPos, cancoderVel); + + /* Assert the two methods match */ + System.out.println("Talon Pos: " + compensatedTalonPos + " - " + functionCompensatedTalon); + System.out.println( + "CANcoder Pos: " + compensatedCANcoderPos + " - " + functionCompensatedCANcoder); + assertEquals(compensatedTalonPos, functionCompensatedTalon, DOUBLE_DELTA); + assertEquals(compensatedCANcoderPos, functionCompensatedCANcoder, DOUBLE_DELTA); + } +} From 04140147137688ceb20e77f1435e87ed74018b78 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 23 Oct 2024 17:13:39 -0700 Subject: [PATCH 33/57] Builds and runs with CRTE swerve library Still need to get the telemetry working... modified: .gitignore modified: src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java renamed: src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java -> src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDrive.java renamed: src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java -> src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java renamed: src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java -> src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteFieldDrive.java modified: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java renamed: src/main/java/frc/robot/subsystems/swervedrive/underlying/YAGSLSwerve.java -> src/main/java/frc/robot/subsystems/swervedrive/yagsl_old/YAGSLSwerve.java modified: src/main/java/frc/robot/subsystems/vision/Vision.java --- .gitignore | 2 + .../swervedrive/auto/AutoBalanceCommand.java | 2 +- ...uteDrive.java => YAGSL_AbsoluteDrive.java} | 6 +- ...veAdv.java => YAGSL_AbsoluteDriveAdv.java} | 6 +- ...ive.java => YAGSL_AbsoluteFieldDrive.java} | 6 +- .../swervedrive/SwerveSubsystem.java | 89 +++++++++++++++++++ .../YAGSLSwerve.java | 12 +-- .../frc/robot/subsystems/vision/Vision.java | 3 +- 8 files changed, 109 insertions(+), 17 deletions(-) rename src/main/java/frc/robot/commands/swervedrive/drivebase/{AbsoluteDrive.java => YAGSL_AbsoluteDrive.java} (97%) rename src/main/java/frc/robot/commands/swervedrive/drivebase/{AbsoluteDriveAdv.java => YAGSL_AbsoluteDriveAdv.java} (97%) rename src/main/java/frc/robot/commands/swervedrive/drivebase/{AbsoluteFieldDrive.java => YAGSL_AbsoluteFieldDrive.java} (96%) rename src/main/java/frc/robot/subsystems/swervedrive/{underlying => yagsl_old}/YAGSLSwerve.java (99%) diff --git a/.gitignore b/.gitignore index e144021e..a9228987 100644 --- a/.gitignore +++ b/.gitignore @@ -171,6 +171,8 @@ out/ # Simulation GUI and other tools window save file simgui*.json +ctre_sim +networktables.json # Version file src/main/java/frc/robot/BuildConstants.java diff --git a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java b/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java index 013021c7..8baeed40 100644 --- a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java +++ b/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java @@ -24,7 +24,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.swervedrive.underlying.YAGSLSwerve; +import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; /** * Auto Balance command using a simple PID controller. Created by Team 3512 Date: Wed, 30 Oct 2024 11:03:02 -0700 Subject: [PATCH 34/57] Plumbed in accelerometers to logging They run as a Virtual Subsystem, logging each "click". modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java deleted: src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIO.java deleted: src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIOPigeon2.java deleted: src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIORio.java modified: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java deleted: vendordeps/Phoenix5.json --- src/main/java/frc/robot/Constants.java | 25 ++- src/main/java/frc/robot/RobotContainer.java | 5 + .../accelerometer/Accelerometer.java | 71 ++++++-- .../accelerometer/AccelerometerIO.java | 44 ----- .../accelerometer/AccelerometerIOPigeon2.java | 42 ----- .../accelerometer/AccelerometerIORio.java | 16 -- .../swervedrive/SwerveSubsystem.java | 3 +- vendordeps/Phoenix5.json | 151 ------------------ 8 files changed, 88 insertions(+), 269 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIO.java delete mode 100644 src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIOPigeon2.java delete mode 100644 src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIORio.java delete mode 100644 vendordeps/Phoenix5.json diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 578bc871..608f4d25 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,6 +22,7 @@ import com.pathplanner.lib.util.PIDConstants; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Filesystem; @@ -102,8 +103,8 @@ public static void main(String... args) { /** Enumerate the supported swerve drive types */ public static enum SwerveType { - YAGSL, // The generic YAGSL swerve library - PHOENIX6 // The all-CTRE Phoenix6 swerve library + PHOENIX6, // The all-CTRE Phoenix6 swerve library + YAGSL // The generic YAGSL swerve library (not supported at this time) } /** Get the current swerve drive type */ @@ -119,6 +120,26 @@ public static SwerveType getSwerve() { public static final boolean tuningMode = false; + /** Accelerometer Constants ********************************************** */ + public static class AccelerometerConstants { + // Insert here the orientation (CCW == +) of the Rio and Pigeon2 from the robot + // An angle of "0." means the x-y-z markings on the device match the robot's intrinsic reference + // frame. + // NOTE: It is assumed that both the Rio and the Pigeon are mounted such that +Z is UP + public static final Rotation2d kRioOrientation = + switch (getRobot()) { + case COMPBOT -> Rotation2d.fromDegrees(0.); + case DEVBOT -> Rotation2d.fromDegrees(0.); + default -> Rotation2d.fromDegrees(0.); + }; + public static final Rotation2d kPigeonOrientation = + switch (getRobot()) { + case COMPBOT -> Rotation2d.fromDegrees(0.); + case DEVBOT -> Rotation2d.fromDegrees(0.); + default -> Rotation2d.fromDegrees(0.); + }; + } + /** Physical Constants for Robot Operation ******************************* */ public static final class PhysicalConstants { public static final double kRobotMass = (148 - 20.3) * 0.453592; // 32lbs * kg per pound diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4890886f..67e3bd95 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,6 +29,7 @@ import frc.robot.Constants.AprilTagConstants; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.accelerometer.Accelerometer; import frc.robot.subsystems.flywheel_example.Flywheel; import frc.robot.subsystems.flywheel_example.FlywheelIO; import frc.robot.subsystems.flywheel_example.FlywheelIOSim; @@ -52,6 +53,7 @@ public class RobotContainer { private final SwerveSubsystem m_drivebase; private final Flywheel m_flywheel; private final Vision m_vision; + private final Accelerometer m_accel; // Dashboard inputs private final LoggedDashboardChooser autoChooser; @@ -79,6 +81,7 @@ public RobotContainer() { new Vision( this::getAprilTagLayoutType, new VisionIOPhoton(this::getAprilTagLayoutType, "Photon_CAMNAME")); + m_accel = new Accelerometer(m_drivebase.getPigeon2()); break; case SIM: @@ -86,6 +89,7 @@ public RobotContainer() { m_drivebase = TunerConstants.DriveTrain; m_flywheel = new Flywheel(new FlywheelIOSim()); m_vision = new Vision(this::getAprilTagLayoutType); + m_accel = new Accelerometer(m_drivebase.getPigeon2()); break; default: @@ -93,6 +97,7 @@ public RobotContainer() { m_drivebase = TunerConstants.DriveTrain; m_flywheel = new Flywheel(new FlywheelIO() {}); m_vision = new Vision(this::getAprilTagLayoutType, new VisionIO() {}, new VisionIO() {}); + m_accel = new Accelerometer(m_drivebase.getPigeon2()); break; } diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 9ab29557..85870587 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -13,27 +13,72 @@ package frc.robot.subsystems.accelerometer; -import frc.robot.subsystems.accelerometer.AccelerometerIO.AccelerometerIOInputs; +import static frc.robot.Constants.AccelerometerConstants.*; + +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.BuiltInAccelerometer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants; import frc.robot.util.VirtualSubsystem; +import org.littletonrobotics.junction.Logger; /** Accelerometer subsystem (built upon a virtual subsystem) */ public class Accelerometer extends VirtualSubsystem { - private final AccelerometerIO[] io; - private final AccelerometerIOInputs[] inputs; + private final BuiltInAccelerometer rioAccelerometer = new BuiltInAccelerometer(); + private final Pigeon2 pigeonAccelerometer; + + // Define the 3D vectors needed to hold values + private Translation3d rioAccVector; + private Translation3d pigeonAccVector; + private Translation3d prevRioAccel = new Translation3d(); + private Translation3d prevPigeonAccel = new Translation3d(); + private Translation3d rioJerkVector; + private Translation3d pigeonJerkVector; - // Class method definition, including inputs - public Accelerometer(AccelerometerIO... io) { - this.io = io; - inputs = new AccelerometerIOInputs[io.length]; - for (int i = 0; i < io.length; i++) { - inputs[i] = new AccelerometerIOInputs(); - } + // Class method definition + public Accelerometer(Pigeon2 pigeonAccelerometer) { + this.pigeonAccelerometer = pigeonAccelerometer; } + /** Get accelerations, compute jerks, log everything */ public void periodic() { - for (int i = 0; i < io.length; i++) { - io[i].updateInputs(inputs[i]); - } + + // Compute the Rio's acceleration, rotated as needed in the XY plane (yaw) + // RoboRio provides accelerations in `g`, from -8 to +8; convert to m/s^2 + rioAccVector = + new Translation3d(rioAccelerometer.getX(), rioAccelerometer.getY(), rioAccelerometer.getZ()) + .rotateBy(new Rotation3d(0., 0., kRioOrientation.getRadians())) + .times(9.81); + + // Compute the Pigeon's acceleration, rotated as needed + // Pigeon provides accelerations in `g`, from -2 to +2; convert to m/s^2 + pigeonAccVector = + new Translation3d( + pigeonAccelerometer.getAccelerationX().getValueAsDouble(), + pigeonAccelerometer.getAccelerationY().getValueAsDouble(), + pigeonAccelerometer.getAccelerationZ().getValueAsDouble()) + .rotateBy(new Rotation3d(0., 0., kPigeonOrientation.getRadians())) + .times(9.81); + + // Compute the jerks ((current - prev accel) / loop time) + rioJerkVector = rioAccVector.minus(prevRioAccel).div(Constants.loopPeriodSecs); + pigeonJerkVector = pigeonAccVector.minus(prevPigeonAccel).div(Constants.loopPeriodSecs); + + // Log everything to both AdvantageKit and SmartDashboard + Logger.recordOutput("Acceleration/Rio/Accel", rioAccVector); + Logger.recordOutput("Acceleration/Rio/Jerk", rioJerkVector); + Logger.recordOutput("Acceleration/Pigeon/Accel", pigeonAccVector); + Logger.recordOutput("Acceleration/Pigeon/Jerk", pigeonJerkVector); + SmartDashboard.putNumber("RioXAccel", rioAccVector.getX()); + SmartDashboard.putNumber("RioYAccel", rioAccVector.getY()); + SmartDashboard.putNumber("PigeonXAccel", pigeonAccVector.getX()); + SmartDashboard.putNumber("PigeonYAccel", pigeonAccVector.getY()); + + // Set the "previous" accelerations to the current for the next loop + prevRioAccel = rioAccVector; + prevPigeonAccel = pigeonAccVector; } } diff --git a/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIO.java b/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIO.java deleted file mode 100644 index f31132f7..00000000 --- a/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIO.java +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.subsystems.accelerometer; - -import org.littletonrobotics.junction.AutoLog; -import org.littletonrobotics.junction.LogTable; -import org.littletonrobotics.junction.inputs.LoggableInputs; - -public interface AccelerometerIO { - @AutoLog - class AccelerometerIOInputs implements LoggableInputs { - public boolean connected = false; - public double accelXVal = 0.0; - public double accelYVal = 0.0; - public double accelZVal = 0.0; - - @Override - public void toLog(LogTable table) { - table.put("AccelX", accelXVal); - table.put("AccelY", accelYVal); - table.put("AccelZ", accelZVal); - } - - @Override - public void fromLog(LogTable table) { - accelXVal = table.get("AccelX", 0.0); - accelYVal = table.get("AccelY", 0.0); - accelZVal = table.get("AccelZ", 0.0); - } - } - - default void updateInputs(AccelerometerIOInputs inputs) {} -} diff --git a/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIOPigeon2.java b/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIOPigeon2.java deleted file mode 100644 index f09d3299..00000000 --- a/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIOPigeon2.java +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.subsystems.accelerometer; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.hardware.Pigeon2; - -public class AccelerometerIOPigeon2 implements AccelerometerIO { - - public final String accelName = "Pigeon2"; - private final Pigeon2 pigeon = new Pigeon2(20); - private final StatusSignal accelX = pigeon.getAccelerationX(); - private final StatusSignal accelY = pigeon.getAccelerationY(); - private final StatusSignal accelZ = pigeon.getAccelerationZ(); - - public AccelerometerIOPigeon2() { - accelX.setUpdateFrequency(100.0); - accelY.setUpdateFrequency(100.0); - accelZ.setUpdateFrequency(100.0); - pigeon.optimizeBusUtilization(); - } - - public void updateInputs(AccelerometerIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(accelX, accelY, accelZ).equals(StatusCode.OK); - inputs.accelXVal = accelX.getValueAsDouble(); - inputs.accelYVal = accelY.getValueAsDouble(); - inputs.accelZVal = accelZ.getValueAsDouble(); - } -} diff --git a/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIORio.java b/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIORio.java deleted file mode 100644 index b449d5b3..00000000 --- a/src/main/java/frc/robot/subsystems/accelerometer/AccelerometerIORio.java +++ /dev/null @@ -1,16 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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 frc.robot.subsystems.accelerometer; - -public class AccelerometerIORio {} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index aafbc6ef..80a619c0 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -236,6 +236,7 @@ public void periodic() { } /** Log Telemetry Data to AdvantageKit */ + // NOTE: Phoenix6 telemetry NOT in SwerveDriveTelemetry!!! Logger.recordOutput("SwerveDive/Telemetry/moduleCount", SwerveDriveTelemetry.moduleCount); Logger.recordOutput("SwerveDive/Telemetry/wheelLocations", SwerveDriveTelemetry.wheelLocations); Logger.recordOutput("SwerveDive/Telemetry/measuredStates", SwerveDriveTelemetry.measuredStates); @@ -265,7 +266,7 @@ public void periodic() { } /************************************************************************* */ - /* COMMAND SECTION -- Swerve-only Commands */ + /* COMMAND SECTION -- Drivebase-only Commands */ /** * Get the path follower with events. diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json deleted file mode 100644 index adf6e02a..00000000 --- a/vendordeps/Phoenix5.json +++ /dev/null @@ -1,151 +0,0 @@ -{ - "fileName": "Phoenix5.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.33.1", - "frcYear": 2024, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", - "requires": [ - { - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", - "offlineFileName": "Phoenix6.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.33.1" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.33.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.33.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.33.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.33.1", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.33.1", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.33.1", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "5.33.1", - "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.33.1", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.33.1", - "libName": "CTRE_PhoenixCCISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} From 2ce0bdc15c796fbca3a1f55ef06f2927835d9211 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 30 Oct 2024 12:15:31 -0700 Subject: [PATCH 35/57] Progress on the CTRE telemetry front new file: src/main/deploy/pathplanner/Tests.path new file: src/main/deploy/pathplanner/autos/Tests.auto new file: src/main/deploy/pathplanner/paths/SimplePath.path modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/RobotContainer.java new file: src/main/java/frc/robot/Telemetry.java modified: src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java modified: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java modified: src/main/java/frc/robot/subsystems/swervedrive/yagsl_old/YAGSLSwerve.java --- src/main/deploy/pathplanner/Tests.path | 234 ++++++++++++++++++ src/main/deploy/pathplanner/autos/Tests.auto | 24 ++ .../deploy/pathplanner/paths/SimplePath.path | 64 +++++ src/main/java/frc/robot/Constants.java | 47 ++-- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 43 +++- src/main/java/frc/robot/Telemetry.java | 124 ++++++++++ .../drivebase/YAGSL_AbsoluteDriveAdv.java | 2 +- .../flywheel_example/FlywheelIOSparkMax.java | 6 +- .../flywheel_example/FlywheelIOTalonFX.java | 5 +- .../swervedrive/SwerveSubsystem.java | 116 ++++----- .../swervedrive/yagsl_old/YAGSLSwerve.java | 19 +- 12 files changed, 591 insertions(+), 95 deletions(-) create mode 100644 src/main/deploy/pathplanner/Tests.path create mode 100644 src/main/deploy/pathplanner/autos/Tests.auto create mode 100644 src/main/deploy/pathplanner/paths/SimplePath.path create mode 100644 src/main/java/frc/robot/Telemetry.java diff --git a/src/main/deploy/pathplanner/Tests.path b/src/main/deploy/pathplanner/Tests.path new file mode 100644 index 00000000..9cb675ba --- /dev/null +++ b/src/main/deploy/pathplanner/Tests.path @@ -0,0 +1,234 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 7.855128995972278, + "y": 3.8506658441888315 + }, + "prevControl": null, + "nextControl": { + "x": 9.51563917413549, + "y": 3.864980587104032 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 9.673101346202692, + "y": 4.909956819913639 + }, + "prevControl": { + "x": 9.658786603287492, + "y": 4.2657933887296355 + }, + "nextControl": { + "x": 9.658786603287492, + "y": 4.2657933887296355 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 9.744675060778693, + "y": 3.278076127580827 + }, + "prevControl": { + "x": 9.701730832033093, + "y": 3.922239558764832 + }, + "nextControl": { + "x": 9.701730832033093, + "y": 3.922239558764832 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.855128995972278, + "y": 3.8506658441888315 + }, + "prevControl": { + "x": 9.071882143764288, + "y": 3.879295330019232 + }, + "nextControl": { + "x": 9.071882143764288, + "y": 3.879295330019232 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 9.673101346202692, + "y": 4.89564207699844 + }, + "prevControl": { + "x": 9.687416089117892, + "y": 4.222849159984035 + }, + "nextControl": { + "x": 9.687416089117892, + "y": 4.222849159984035 + }, + "holonomicAngle": 125.92486296979446, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 9.730360317863493, + "y": 3.263761384665627 + }, + "prevControl": { + "x": 9.748383531854783, + "y": 4.107878917512221 + }, + "nextControl": { + "x": 9.748383531854783, + "y": 4.107878917512221 + }, + "holonomicAngle": -91.03738139006352, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 9.013854168059215, + "y": 3.3824178174672146 + }, + "prevControl": { + "x": 9.077332014313154, + "y": 4.307380720024598 + }, + "nextControl": { + "x": 9.077332014313154, + "y": 4.307380720024598 + }, + "holonomicAngle": 59.62087398863161, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 8.315597859265894, + "y": 3.3461447624649643 + }, + "prevControl": { + "x": 8.415348760522082, + "y": 3.863035796247031 + }, + "nextControl": { + "x": 8.415348760522082, + "y": 3.863035796247031 + }, + "holonomicAngle": -155.7255588655606, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.84404814423664, + "y": 3.863035796247031 + }, + "prevControl": { + "x": 8.78714757429515, + "y": 3.8086262137436555 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "isReversed": null, + "markers": [ + { + "position": 3.4181818181818175, + "names": [ + "marker" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Tests.auto b/src/main/deploy/pathplanner/autos/Tests.auto new file mode 100644 index 00000000..feb22dfb --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Tests.auto @@ -0,0 +1,24 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 4.061722123444247, + "y": 5.038789506150441 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SimplePath" + } + } + ] + } + }, + "folder": null +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SimplePath.path b/src/main/deploy/pathplanner/paths/SimplePath.path new file mode 100644 index 00000000..d070be0e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SimplePath.path @@ -0,0 +1,64 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.061722123444247, + "y": 5.038789506150441 + }, + "prevControl": null, + "nextControl": { + "x": 4.204869552596248, + "y": 4.652291447440038 + }, + "isLocked": false + }, + { + "anchor": { + "x": 5.292790014151457, + "y": 4.494829275372837 + }, + "prevControl": { + "x": 4.5197938967306515, + "y": 4.4805145324576365 + }, + "nextControl": { + "x": 5.979928957731629, + "y": 4.507554070624321 + }, + "isLocked": false + }, + { + "anchor": { + "x": 6.151674589063464, + "y": 5.2964548786240435 + }, + "prevControl": { + "x": 6.065786131572263, + "y": 4.80975361950724 + }, + "nextControl": null, + "isLocked": false + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3, + "maxAcceleration": 3, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0 + }, + "reversed": false, + "folder": null +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 608f4d25..172ac917 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.RobotBase; +import frc.robot.generated.TunerConstants; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; import java.io.IOException; @@ -172,36 +173,39 @@ public static final class PowerConstants { public static final class AutonConstants { // Translation PID constants - public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0); + public static final PIDConstants kAutoTranslatePID = new PIDConstants(0.7, 0, 0); // Rotation PID constants - public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01); + public static final PIDConstants kAutoAnglePID = new PIDConstants(0.4, 0, 0.01); } /** Drive Base Constants ************************************************* */ public static final class DrivebaseConstants { // Physical size of the drive base - private static final double TRACK_WIDTH_X = Units.inchesToMeters(20.75); - private static final double TRACK_WIDTH_Y = Units.inchesToMeters(20.75); - public static final double DRIVE_BASE_RADIUS = - Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); - - // Maximum chassis speeds desired for robot motion -- metric / radians - public static final double MAX_LINEAR_SPEED = Units.feetToMeters(18); // ft/s - public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; + private static final double kTrackWidthX = Units.inchesToMeters(20.75); + private static final double kTrackWidthY = Units.inchesToMeters(20.75); + public static final double kDriveBaseRadius = + Math.hypot(kTrackWidthX / 2.0, kTrackWidthY / 2.0); + + // CTRE-based maximum robot speeds (adjustable in Phoenix X Tuner Swerve Generator) + // kSpeedAt12VoltsMps desired top speed + public static final double kMaxLinearSpeed = TunerConstants.kSpeedAt12VoltsMps; + // 3/4 of a rotation per second max angular velocity + public static final double kMaxAngularSpeed = 1.5 * Math.PI; + // Maximum chassis accelerations desired for robot motion -- metric / radians - public static final double MAX_LINEAR_ACCEL = 4.0; // m/s/s - public static final double MAX_ANGULAR_ACCEL = Units.degreesToRadians(720); // deg/s/s + public static final double kMaxLinearAccel = 4.0; // m/s/s + public static final double kMaxAngularAccel = Units.degreesToRadians(720); // deg/s/s // Wheel radius - public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); + public static final double kWheelRadius = Units.inchesToMeters(2.0); // ** Gear ratios for SDS MK4i L2, adjust as necessary ** - public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - public static final double TURN_GEAR_RATIO = 150.0 / 7.0; + public static final double kDriveGearRatio = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + public static final double kTurnGearRatio = 150.0 / 7.0; // Hold time on motor brakes when disabled - public static final double WHEEL_LOCK_TIME = 10; // seconds + public static final double kWheelLockTime = 10; // seconds // SysID characterization constants public static final double kMaxV = 12.0; // Max volts @@ -214,7 +218,7 @@ public static final class DrivebaseConstants { public static final class FlywheelConstants { // Mechanism motor gear ratio - public static final double GEAR_RATIO = 1.5; + public static final double kFlywheelGearRatio = 1.5; // MODE == REAL / REPLAY // Feedforward constants @@ -239,10 +243,11 @@ public static final class FlywheelConstants { public static class OperatorConstants { // Joystick Deadband - public static final double LEFT_X_DEADBAND = 0.1; - public static final double LEFT_Y_DEADBAND = 0.1; - public static final double RIGHT_X_DEADBAND = 0.1; - public static final double TURN_CONSTANT = 6; + public static final double kLeftXDeadband = 0.1; + public static final double kLeftYDeadband = 0.1; + public static final double kRightXDeadband = 0.1; + public static final double kRightYDeadband = 0.1; + public static final double kTurnConstant = 6; } /** AprilTag Field Layout ************************************************ */ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 43fd44e5..fdb1de8b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -134,7 +134,7 @@ public void disabledInit() { @Override public void disabledPeriodic() { // After WHEEL_LOCK_TIME has elapsed, release the drive brakes - if (m_disabledTimer.hasElapsed(Constants.DrivebaseConstants.WHEEL_LOCK_TIME)) { + if (m_disabledTimer.hasElapsed(Constants.DrivebaseConstants.kWheelLockTime)) { m_robotContainer.setMotorBrake(false); m_disabledTimer.stop(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 67e3bd95..5affccb9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,8 @@ package frc.robot; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose3d; @@ -28,6 +30,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.AprilTagConstants; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; +import frc.robot.Constants.DrivebaseConstants; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.accelerometer.Accelerometer; import frc.robot.subsystems.flywheel_example.Flywheel; @@ -112,11 +115,49 @@ public RobotContainer() { } /** Use this method to define your TeleOp commands. */ - private void defineTeleopCommands() {} + private void defineTeleopCommands() { + + // Field-centric driving in open loop mode + final SwerveRequest.FieldCentric drive = + new SwerveRequest.FieldCentric() + .withDeadband(DrivebaseConstants.kMaxLinearSpeed * 0.1) + .withRotationalDeadband(DrivebaseConstants.kMaxAngularSpeed * 0.1) // Add a 10% deadband + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + final SwerveRequest.RobotCentric forwardStraight = + new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + + m_drivebase.setDefaultCommand( // Drivetrain will execute this command periodically + m_drivebase + .applyRequest( + () -> + drive + .withVelocityX( + -driverXbox.getLeftY() + * DrivebaseConstants.kMaxLinearSpeed) // Drive forward with + // negative Y (forward) + .withVelocityY( + -driverXbox.getLeftX() + * DrivebaseConstants + .kMaxLinearSpeed) // Drive left with negative X (left) + .withRotationalRate( + -driverXbox.getRightX() + * DrivebaseConstants + .kMaxAngularSpeed) // Drive counterclockwise with negative X + // (left) + ) + .ignoringDisable(true)); + } /** Use this method to define your Autonomous commands for use with PathPlanner / Choreo */ private void defineAutoCommands() { + Command runAuto = m_drivebase.getAutoPath("Tests"); + + final Telemetry logger = new Telemetry(DrivebaseConstants.kMaxLinearSpeed); + NamedCommands.registerCommand("Zero", Commands.runOnce(() -> m_drivebase.tareEverything())); } diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java new file mode 100644 index 00000000..507dfe2f --- /dev/null +++ b/src/main/java/frc/robot/Telemetry.java @@ -0,0 +1,124 @@ +package frc.robot; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class Telemetry { + private final double MaxSpeed; + + /** + * Construct a telemetry object, with the specified max speed of the robot + * + * @param maxSpeed Maximum speed in meters per second + */ + public Telemetry(double maxSpeed) { + MaxSpeed = maxSpeed; + SignalLogger.start(); + } + + /* What to publish over networktables for telemetry */ + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + + /* Robot pose for field positioning */ + private final NetworkTable table = inst.getTable("Pose"); + private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + + /* Robot speeds for general checking */ + private final NetworkTable driveStats = inst.getTable("Drive"); + private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish(); + private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish(); + private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish(); + private final DoublePublisher odomFreq = + driveStats.getDoubleTopic("Odometry Frequency").publish(); + + /* Keep a reference of the last pose to calculate the speeds */ + private Pose2d m_lastPose = new Pose2d(); + private double lastTime = Utils.getCurrentTimeSeconds(); + + /* Mechanisms to represent the swerve module states */ + private final Mechanism2d[] m_moduleMechanisms = + new Mechanism2d[] { + new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), + }; + /* A direction and length changing ligament for speed representation */ + private final MechanismLigament2d[] m_moduleSpeeds = + new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + }; + /* A direction changing and length constant ligament for module direction */ + private final MechanismLigament2d[] m_moduleDirections = + new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; + + /* Accept the swerve drive state and telemeterize it to smartdashboard */ + public void telemeterize(SwerveDriveState state) { + /* Telemeterize the pose */ + Pose2d pose = state.Pose; + fieldTypePub.set("Field2d"); + fieldPub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); + + /* Telemeterize the robot's general speeds */ + double currentTime = Utils.getCurrentTimeSeconds(); + double diffTime = currentTime - lastTime; + lastTime = currentTime; + Translation2d distanceDiff = pose.minus(m_lastPose).getTranslation(); + m_lastPose = pose; + + Translation2d velocities = distanceDiff.div(diffTime); + + speed.set(velocities.getNorm()); + velocityX.set(velocities.getX()); + velocityY.set(velocities.getY()); + odomFreq.set(1.0 / state.OdometryPeriod); + + /* Telemeterize the module's states */ + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + + SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); + } + + SignalLogger.writeDoubleArray( + "odometry", new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); + SignalLogger.writeDouble("odom period", state.OdometryPeriod, "seconds"); + } +} diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java index 139a2007..c787a547 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java @@ -148,7 +148,7 @@ public void execute() { if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { resetHeading = true; swerve.drive( - translation, (OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), true); + translation, (OperatorConstants.kTurnConstant * -headingAdjust.getAsDouble()), true); } else { swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); } diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java index 5c6c9797..cf9b845d 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java @@ -57,9 +57,9 @@ public FlywheelIOSparkMax() { @Override public void updateInputs(FlywheelIOInputs inputs) { - inputs.positionRad = Units.rotationsToRadians(encoder.getPosition() / GEAR_RATIO); + inputs.positionRad = Units.rotationsToRadians(encoder.getPosition() / kFlywheelGearRatio); inputs.velocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / GEAR_RATIO); + Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / kFlywheelGearRatio); inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage(); inputs.currentAmps = new double[] {leader.getOutputCurrent(), follower.getOutputCurrent()}; } @@ -72,7 +72,7 @@ public void setVoltage(double volts) { @Override public void setVelocity(double velocityRadPerSec, double ffVolts) { pid.setReference( - Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO, + Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * kFlywheelGearRatio, ControlType.kVelocity, 0, ffVolts, diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java index b5f99f91..2e087214 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java @@ -60,9 +60,10 @@ public FlywheelIOTalonFX() { public void updateInputs(FlywheelIOInputs inputs) { BaseStatusSignal.refreshAll( leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); - inputs.positionRad = Units.rotationsToRadians(leaderPosition.getValueAsDouble()) / GEAR_RATIO; + inputs.positionRad = + Units.rotationsToRadians(leaderPosition.getValueAsDouble()) / kFlywheelGearRatio; inputs.velocityRadPerSec = - Units.rotationsToRadians(leaderVelocity.getValueAsDouble()) / GEAR_RATIO; + Units.rotationsToRadians(leaderVelocity.getValueAsDouble()) / kFlywheelGearRatio; inputs.appliedVolts = leaderAppliedVolts.getValueAsDouble(); inputs.currentAmps = new double[] {leaderCurrent.getValueAsDouble(), followerCurrent.getValueAsDouble()}; diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 80a619c0..c6a67e05 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -40,7 +40,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; @@ -53,7 +52,6 @@ import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import swervelib.telemetry.SwerveDriveTelemetry; /** * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used @@ -131,6 +129,7 @@ public SwerveSubsystem( if (Utils.isSimulation()) { startSimThread(); } + Logger.recordOutput("SwerveDrive/Comment1", "This is a comment!"); } public SwerveSubsystem( @@ -140,6 +139,7 @@ public SwerveSubsystem( if (Utils.isSimulation()) { startSimThread(); } + Logger.recordOutput("SwerveDrive/Comment2", "This is a comment!"); } private void configurePathPlanner() { @@ -236,33 +236,38 @@ public void periodic() { } /** Log Telemetry Data to AdvantageKit */ - // NOTE: Phoenix6 telemetry NOT in SwerveDriveTelemetry!!! - Logger.recordOutput("SwerveDive/Telemetry/moduleCount", SwerveDriveTelemetry.moduleCount); - Logger.recordOutput("SwerveDive/Telemetry/wheelLocations", SwerveDriveTelemetry.wheelLocations); - Logger.recordOutput("SwerveDive/Telemetry/measuredStates", SwerveDriveTelemetry.measuredStates); - Logger.recordOutput("SwerveDive/Telemetry/desiredStates", SwerveDriveTelemetry.desiredStates); - Logger.recordOutput("SwerveDive/Telemetry/robotRotation", SwerveDriveTelemetry.robotRotation); - Logger.recordOutput("SwerveDive/Telemetry/maxSpeed", SwerveDriveTelemetry.maxSpeed); - Logger.recordOutput("SwerveDive/Telemetry/rotationUnit", SwerveDriveTelemetry.rotationUnit); - Logger.recordOutput("SwerveDive/Telemetry/sizeLeftRight", SwerveDriveTelemetry.sizeLeftRight); - Logger.recordOutput("SwerveDive/Telemetry/sizeFrontBack", SwerveDriveTelemetry.sizeFrontBack); - Logger.recordOutput( - "SwerveDive/Telemetry/forwardDirection", SwerveDriveTelemetry.forwardDirection); - Logger.recordOutput( - "SwerveDive/Telemetry/maxAngularVelocity", SwerveDriveTelemetry.maxAngularVelocity); - Logger.recordOutput( - "SwerveDive/Telemetry/measuredChassisSpeeds", SwerveDriveTelemetry.measuredChassisSpeeds); - Logger.recordOutput( - "SwerveDive/Telemetry/desiredChassisSpeeds", SwerveDriveTelemetry.desiredChassisSpeeds); - - /** Log Swerve Drive States to AdvantageKit */ - getModuleStates(); - getDesiredStates(); - Logger.recordOutput( - "SwerveDive/States/RobotRotation", - SwerveDriveTelemetry.rotationUnit == "degrees" - ? Rotation2d.fromDegrees(SwerveDriveTelemetry.robotRotation) - : Rotation2d.fromRadians(SwerveDriveTelemetry.robotRotation)); + Logger.recordOutput("SwerveDrive/Comment", "This is a comment!"); + // Logger.recordOutput("SwerveDrive/Telemetry/moduleCount", this.ModuleCount); + // Logger.recordOutput("SwerveDrive/Telemetry/wheelLocations", this.m_moduleLocations); + Logger.recordOutput("SwerveDrive/Telemetry/measuredStates", getState().ModuleStates); + // Logger.recordOutput("SwerveDive/Telemetry/desiredStates", + // SwerveDriveTelemetry.desiredStates); + // Logger.recordOutput("SwerveDive/Telemetry/robotRotation", + // SwerveDriveTelemetry.robotRotation); + // Logger.recordOutput("SwerveDive/Telemetry/maxSpeed", SwerveDriveTelemetry.maxSpeed); + // Logger.recordOutput("SwerveDive/Telemetry/rotationUnit", SwerveDriveTelemetry.rotationUnit); + // Logger.recordOutput("SwerveDive/Telemetry/sizeLeftRight", + // SwerveDriveTelemetry.sizeLeftRight); + // Logger.recordOutput("SwerveDive/Telemetry/sizeFrontBack", + // SwerveDriveTelemetry.sizeFrontBack); + // Logger.recordOutput( + // "SwerveDive/Telemetry/forwardDirection", SwerveDriveTelemetry.forwardDirection); + // Logger.recordOutput( + // "SwerveDive/Telemetry/maxAngularVelocity", SwerveDriveTelemetry.maxAngularVelocity); + // Logger.recordOutput( + // "SwerveDive/Telemetry/measuredChassisSpeeds", + // SwerveDriveTelemetry.measuredChassisSpeeds); + // Logger.recordOutput( + // "SwerveDive/Telemetry/desiredChassisSpeeds", SwerveDriveTelemetry.desiredChassisSpeeds); + + // /** Log Swerve Drive States to AdvantageKit */ + // getModuleStates(); + // getDesiredStates(); + // Logger.recordOutput( + // "SwerveDive/States/RobotRotation", + // SwerveDriveTelemetry.rotationUnit == "degrees" + // ? Rotation2d.fromDegrees(SwerveDriveTelemetry.robotRotation) + // : Rotation2d.fromRadians(SwerveDriveTelemetry.robotRotation)); } /************************************************************************* */ @@ -288,8 +293,7 @@ public Command getAutonomousCommand(String pathName) { public Command driveToPose(Pose2d pose) { // Create the constraints to use while pathfinding PathConstraints constraints = - new PathConstraints( - MAX_LINEAR_SPEED, MAX_LINEAR_ACCEL, MAX_ANGULAR_SPEED, MAX_ANGULAR_ACCEL); + new PathConstraints(kMaxLinearSpeed, kMaxLinearAccel, kMaxAngularSpeed, kMaxAngularAccel); // Since AutoBuilder is configured, we can use it to build pathfinding commands return AutoBuilder.pathfindToPose( @@ -327,29 +331,29 @@ public void setMotorBrake(boolean brake) { } } - /** Returns the module states (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "SwerveDive/States/Measured") - private SwerveModuleState[] getModuleStates() { - SwerveModuleState[] states = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - states[i] = - new SwerveModuleState( - SwerveDriveTelemetry.measuredStates[(i * 2) + 1], - Rotation2d.fromDegrees(SwerveDriveTelemetry.measuredStates[i * 2])); - } - return states; - } - - /** Returns the desired states (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "SwerveDive/States/Desired") - private SwerveModuleState[] getDesiredStates() { - SwerveModuleState[] states = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - states[i] = - new SwerveModuleState( - SwerveDriveTelemetry.desiredStates[(i * 2) + 1], - Rotation2d.fromDegrees(SwerveDriveTelemetry.desiredStates[i * 2])); - } - return states; - } + // /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + // @AutoLogOutput(key = "SwerveDive/States/Measured") + // private SwerveModuleState[] getModuleStates() { + // SwerveModuleState[] states = new SwerveModuleState[4]; + // for (int i = 0; i < 4; i++) { + // states[i] = + // new SwerveModuleState( + // SwerveDriveTelemetry.measuredStates[(i * 2) + 1], + // Rotation2d.fromDegrees(SwerveDriveTelemetry.measuredStates[i * 2])); + // } + // return states; + // } + + // /** Returns the desired states (turn angles and drive velocities) for all of the modules. */ + // @AutoLogOutput(key = "SwerveDive/States/Desired") + // private SwerveModuleState[] getDesiredStates() { + // SwerveModuleState[] states = new SwerveModuleState[4]; + // for (int i = 0; i < 4; i++) { + // states[i] = + // new SwerveModuleState( + // SwerveDriveTelemetry.desiredStates[(i * 2) + 1], + // Rotation2d.fromDegrees(SwerveDriveTelemetry.desiredStates[i * 2])); + // } + // return states; + // } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/yagsl_old/YAGSLSwerve.java b/src/main/java/frc/robot/subsystems/swervedrive/yagsl_old/YAGSLSwerve.java index c29b365f..7efbee0c 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/yagsl_old/YAGSLSwerve.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/yagsl_old/YAGSLSwerve.java @@ -80,12 +80,12 @@ public class YAGSLSwerve extends SubsystemBase { public YAGSLSwerve(File directory) { // Angle conversion factor is 360 / (GEAR RATIO * ENCODER RESOLUTION) - double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(TURN_GEAR_RATIO); + double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(kTurnGearRatio); // Motor conversion factor is (PI * WHEEL DIAMETER IN METERS) / (GEAR RATIO * ENCODER // RESOLUTION) double driveConversionFactor = - SwerveMath.calculateMetersPerRotation(WHEEL_RADIUS * 2.0, DRIVE_GEAR_RATIO); + SwerveMath.calculateMetersPerRotation(kWheelRadius * 2.0, kDriveGearRatio); // Output to console and to AdvantageKit System.out.println("\"conversionFactors\": {"); @@ -99,7 +99,7 @@ public YAGSLSwerve(File directory) { // created. SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; try { - swerveDrive = new SwerveParser(directory).createSwerveDrive(MAX_LINEAR_SPEED); + swerveDrive = new SwerveParser(directory).createSwerveDrive(kMaxLinearSpeed); // Alternative method if you don't want to supply the conversion factor via JSON files. // swerveDrive = new SwerveParser_RBSI(directory).createSwerveDrive(maximumSpeed, // angleConversionFactor, driveConversionFactor); @@ -142,7 +142,7 @@ public YAGSLSwerve(File directory) { */ public YAGSLSwerve( SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { - swerveDrive = new SwerveDrive(driveCfg, controllerCfg, MAX_LINEAR_SPEED); + swerveDrive = new SwerveDrive(driveCfg, controllerCfg, kMaxLinearSpeed); } /** Setup the photon vision class. */ @@ -158,10 +158,10 @@ public void setupPathPlanner() { this::getRobotVelocity, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE this::setChassisSpeeds, // Driver method given ROBOT RELATIVE ChassisSpeeds new HolonomicPathFollowerConfig( - AutonConstants.TRANSLATION_PID, - AutonConstants.ANGLE_PID, - MAX_LINEAR_SPEED, - DRIVE_BASE_RADIUS, + AutonConstants.kAutoTranslatePID, + AutonConstants.kAutoAnglePID, + kMaxLinearSpeed, + kDriveBaseRadius, new ReplanningConfig()), () -> { // Boolean supplier that controls when the path will be mirrored for the red alliance @@ -363,8 +363,7 @@ public Command getAutonomousCommand(String pathName) { public Command driveToPose(Pose2d pose) { // Create the constraints to use while pathfinding PathConstraints constraints = - new PathConstraints( - MAX_LINEAR_SPEED, MAX_LINEAR_ACCEL, MAX_ANGULAR_SPEED, MAX_ANGULAR_ACCEL); + new PathConstraints(kMaxLinearSpeed, kMaxLinearAccel, kMaxAngularSpeed, kMaxAngularAccel); // Since AutoBuilder is configured, we can use it to build pathfinding commands return AutoBuilder.pathfindToPose( From 0b5e0806fee2202ed982d728e812aaa116275e7c Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 30 Oct 2024 12:55:58 -0700 Subject: [PATCH 36/57] Organizing the commands and whatnot modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java renamed: src/main/java/frc/robot/Telemetry.java -> src/main/java/frc/robot/subsystems/swervedrive/SwerveTelemetry.java --- src/main/java/frc/robot/RobotContainer.java | 130 +++++++----------- .../swervedrive/SwerveSubsystem.java | 15 ++ .../swervedrive/SwerveTelemetry.java} | 26 +++- 3 files changed, 87 insertions(+), 84 deletions(-) rename src/main/java/frc/robot/{Telemetry.java => subsystems/swervedrive/SwerveTelemetry.java} (84%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5affccb9..d382403b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,11 +17,10 @@ package frc.robot; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; @@ -37,6 +36,7 @@ import frc.robot.subsystems.flywheel_example.FlywheelIO; import frc.robot.subsystems.flywheel_example.FlywheelIOSim; import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.subsystems.swervedrive.SwerveTelemetry; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhoton; @@ -47,7 +47,7 @@ public class RobotContainer { // Define the Driver and, optionally, the Operator/Co-Driver Controllers - // Replace with CommandPS4Controller or CommandJoystick if needed + // Replace with ``CommandPS4Controller`` or ``CommandJoystick`` if needed final CommandXboxController driverXbox = new CommandXboxController(0); final CommandXboxController operatorXbox = new CommandXboxController(1); final OverrideSwitches overrides = new OverrideSwitches(2); @@ -61,6 +61,9 @@ public class RobotContainer { // Dashboard inputs private final LoggedDashboardChooser autoChooser; + // Swerve Drive Telemetry + private final SwerveTelemetry logger = new SwerveTelemetry(DrivebaseConstants.kMaxLinearSpeed); + /** Returns the current AprilTag layout type. */ public AprilTagLayoutType getAprilTagLayoutType() { return AprilTagConstants.defaultAprilTagType; @@ -106,58 +109,17 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); - // Define TeleOp commands - defineTeleopCommands(); // Define Auto commands defineAutoCommands(); // Set up the SmartDashboard Auto Chooser autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); - } - - /** Use this method to define your TeleOp commands. */ - private void defineTeleopCommands() { - - // Field-centric driving in open loop mode - final SwerveRequest.FieldCentric drive = - new SwerveRequest.FieldCentric() - .withDeadband(DrivebaseConstants.kMaxLinearSpeed * 0.1) - .withRotationalDeadband(DrivebaseConstants.kMaxAngularSpeed * 0.1) // Add a 10% deadband - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - - final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - final SwerveRequest.RobotCentric forwardStraight = - new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); - final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - - m_drivebase.setDefaultCommand( // Drivetrain will execute this command periodically - m_drivebase - .applyRequest( - () -> - drive - .withVelocityX( - -driverXbox.getLeftY() - * DrivebaseConstants.kMaxLinearSpeed) // Drive forward with - // negative Y (forward) - .withVelocityY( - -driverXbox.getLeftX() - * DrivebaseConstants - .kMaxLinearSpeed) // Drive left with negative X (left) - .withRotationalRate( - -driverXbox.getRightX() - * DrivebaseConstants - .kMaxAngularSpeed) // Drive counterclockwise with negative X - // (left) - ) - .ignoringDisable(true)); + // Set up the logger + m_drivebase.registerTelemetry(logger::telemeterize); } /** Use this method to define your Autonomous commands for use with PathPlanner / Choreo */ private void defineAutoCommands() { - Command runAuto = m_drivebase.getAutoPath("Tests"); - - final Telemetry logger = new Telemetry(DrivebaseConstants.kMaxLinearSpeed); - NamedCommands.registerCommand("Zero", Commands.runOnce(() -> m_drivebase.tareEverything())); } @@ -172,42 +134,48 @@ private void defineAutoCommands() { */ private void configureBindings() { - // Manually Re-Zero the Gyro + // Manually Re-Zero the Gyro & Odometry driverXbox.y().onTrue(Commands.runOnce(() -> m_drivebase.tareEverything())); - // Example button bindings from YAGSL - // if (DriverStation.isTest()) - // { - // driverXbox.b().whileTrue(drivebase.sysIdDriveMotorCommand()); - // driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); - // driverXbox.y().whileTrue(drivebase.driveToDistanceCommand(1.0, 0.2)); - // driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro))); - // driverXbox.back().whileTrue(drivebase.centerModulesCommand()); - // driverXbox.leftBumper().onTrue(Commands.none()); - // driverXbox.rightBumper().onTrue(Commands.none()); - // drivebase.setDefaultCommand( - // !RobotBase.isSimulation() ? driveFieldOrientedAnglularVelocity : - // driveFieldOrientedDirectAngleSim); - // } else - // { - // driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro))); - // driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading)); - // driverXbox.b().whileTrue( - // Commands.deferredProxy(() -> drivebase.driveToPose( - // new Pose2d(new Translation2d(4, 4), - // Rotation2d.fromDegrees(0))) - // )); - // driverXbox.y().whileTrue(drivebase.aimAtSpeaker(2)); - // driverXbox.start().whileTrue(Commands.none()); - // driverXbox.back().whileTrue(Commands.none()); - // driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, - // drivebase).repeatedly()); - // driverXbox.rightBumper().onTrue(Commands.none()); - // drivebase.setDefaultCommand( - // !RobotBase.isSimulation() ? driveFieldOrientedDirectAngle : - // driveFieldOrientedDirectAngleSim); - // } - + // Example Swerve Drive button bindings + // A -> BRAKE + driverXbox.a().whileTrue(m_drivebase.applyRequest(() -> m_drivebase.brake)); + // B -> POINT WHEELS AT DIRECTION WITHOUT MOVING + driverXbox + .b() + .whileTrue( + m_drivebase.applyRequest( + () -> + m_drivebase.point.withModuleDirection( + new Rotation2d(-driverXbox.getLeftY(), -driverXbox.getLeftX())))); + // LEFT BUMPER -> reset the field-centric heading + driverXbox.leftBumper().onTrue(m_drivebase.runOnce(() -> m_drivebase.seedFieldRelative())); + // POV UP -> DRIVE FORWARD IN ROBOT-CENTRIC MODE + driverXbox + .pov(0) + .whileTrue( + m_drivebase.applyRequest( + () -> m_drivebase.forwardStraight.withVelocityX(0.5).withVelocityY(0))); + // POV DOWN -> DRIVE BACKWARD IN ROBOT-CENTRIC MODE + driverXbox + .pov(180) + .whileTrue( + m_drivebase.applyRequest( + () -> m_drivebase.forwardStraight.withVelocityX(-0.5).withVelocityY(0))); + + // SET STANDARD DRIVING AS DEFAULT COMMAND FOR THE DRIVEBASE + // NOTE: Left joystick controls lateral translation, right joystick (X) controls rotation + m_drivebase.setDefaultCommand( + m_drivebase + .applyRequest( + () -> + m_drivebase + .drive + .withVelocityX(-driverXbox.getLeftY() * DrivebaseConstants.kMaxLinearSpeed) + .withVelocityY(-driverXbox.getLeftX() * DrivebaseConstants.kMaxLinearSpeed) + .withRotationalRate( + -driverXbox.getRightX() * DrivebaseConstants.kMaxAngularSpeed)) + .ignoringDisable(true)); } /** @@ -218,7 +186,7 @@ private void configureBindings() { public Command getAutonomousCommand() { // An example command will be run in autonomous - return m_drivebase.getAutonomousCommand("New Auto"); + return m_drivebase.getAutoPath("Tests"); // Use the ``autoChooser`` to define your auto path from the SmartDashboard // return autoChooser.get(); } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index c6a67e05..3a9bebb5 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -27,6 +27,7 @@ import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -47,6 +48,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants.DrivebaseConstants; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.vision.Vision; import java.util.function.Supplier; @@ -169,6 +171,19 @@ private void configurePathPlanner() { this); // Subsystem for requirements } + /* Section for defining TeleOp Commands */ + // Field-centric driving in open loop mode + public final SwerveRequest.FieldCentric drive = + new SwerveRequest.FieldCentric() + .withDeadband(DrivebaseConstants.kMaxLinearSpeed * 0.1) + .withRotationalDeadband(DrivebaseConstants.kMaxAngularSpeed * 0.1) // Add a 10% deadband + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + public final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + public final SwerveRequest.RobotCentric forwardStraight = + new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + public final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + public Command applyRequest(Supplier requestSupplier) { return run(() -> this.setControl(requestSupplier.get())); } diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveTelemetry.java similarity index 84% rename from src/main/java/frc/robot/Telemetry.java rename to src/main/java/frc/robot/subsystems/swervedrive/SwerveTelemetry.java index 507dfe2f..eadff169 100644 --- a/src/main/java/frc/robot/Telemetry.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveTelemetry.java @@ -1,4 +1,24 @@ -package frc.robot; +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. +// +// NOTE: This module based on the CTRE Phoenix6 examples +// https://github.com/CrossTheRoadElec/Phoenix6-Examples + +package frc.robot.subsystems.swervedrive; import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; @@ -16,7 +36,7 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; -public class Telemetry { +public class SwerveTelemetry { private final double MaxSpeed; /** @@ -24,7 +44,7 @@ public class Telemetry { * * @param maxSpeed Maximum speed in meters per second */ - public Telemetry(double maxSpeed) { + public SwerveTelemetry(double maxSpeed) { MaxSpeed = maxSpeed; SignalLogger.start(); } From c3e5f52a633f91a7db6920197ee004a429aad1b9 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 1 Nov 2024 15:08:59 -0700 Subject: [PATCH 37/57] Add CTRE Phoenix6 Tuner X Swerve Generator modified: src/main/java/frc/robot/generated/TunerConstants.java modified: src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java --- .../frc/robot/generated/TunerConstants.java | 129 +++++++++--------- .../accelerometer/Accelerometer.java | 8 +- 2 files changed, 71 insertions(+), 66 deletions(-) mode change 100644 => 100755 src/main/java/frc/robot/generated/TunerConstants.java diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java old mode 100644 new mode 100755 index e2bb17bb..b68a658e --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -56,23 +56,21 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12v applied output; // This needs to be tuned to your individual robot - public static final double kSpeedAt12VoltsMps = 4.70; + public static final double kSpeedAt12VoltsMps = 5.21; // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.5; + private static final double kCoupleRatio = 3.5714285714285716; - private static final double kDriveGearRatio = 7.363636364; - private static final double kSteerGearRatio = 15.42857143; - private static final double kWheelRadiusInches = - 2.167; // Estimated at first, then fudge-factored to make odom match record + private static final double kDriveGearRatio = 6.122448979591837; + private static final double kSteerGearRatio = 21.428571428571427; + private static final double kWheelRadiusInches = 2; - private static final boolean kSteerMotorReversed = true; private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - private static final String kCANbusName = "rio"; - private static final int kPigeonId = 1; + private static final String kCANbusName = "DriveTrain"; + private static final int kPigeonId = 13; // These are only used for simulation private static final double kSteerInertia = 0.00001; @@ -104,83 +102,90 @@ public class TunerConstants { .withDriveFrictionVoltage(kDriveFrictionVoltage) .withFeedbackSource(SteerFeedbackType.FusedCANcoder) .withCouplingGearRatio(kCoupleRatio) - .withSteerMotorInverted(kSteerMotorReversed) .withDriveMotorInitialConfigs(driveInitialConfigs) .withSteerMotorInitialConfigs(steerInitialConfigs) .withCANcoderInitialConfigs(cancoderInitialConfigs); // Front Left - private static final int kFrontLeftDriveMotorId = 5; - private static final int kFrontLeftSteerMotorId = 4; - private static final int kFrontLeftEncoderId = 2; - private static final double kFrontLeftEncoderOffset = -0.83544921875; + private static final int kFrontLeftDriveMotorId = 1; + private static final int kFrontLeftSteerMotorId = 2; + private static final int kFrontLeftEncoderId = 3; + private static final double kFrontLeftEncoderOffset = -0.303466796875; + private static final boolean kFrontLeftSteerInvert = true; - private static final double kFrontLeftXPosInches = 10.5; - private static final double kFrontLeftYPosInches = 10.5; + private static final double kFrontLeftXPosInches = 10.375; + private static final double kFrontLeftYPosInches = 10.375; // Front Right - private static final int kFrontRightDriveMotorId = 7; - private static final int kFrontRightSteerMotorId = 6; - private static final int kFrontRightEncoderId = 3; - private static final double kFrontRightEncoderOffset = -0.15234375; + private static final int kFrontRightDriveMotorId = 4; + private static final int kFrontRightSteerMotorId = 5; + private static final int kFrontRightEncoderId = 6; + private static final double kFrontRightEncoderOffset = -0.122802734375; + private static final boolean kFrontRightSteerInvert = true; - private static final double kFrontRightXPosInches = 10.5; - private static final double kFrontRightYPosInches = -10.5; + private static final double kFrontRightXPosInches = 10.375; + private static final double kFrontRightYPosInches = -10.375; // Back Left - private static final int kBackLeftDriveMotorId = 1; - private static final int kBackLeftSteerMotorId = 0; - private static final int kBackLeftEncoderId = 0; - private static final double kBackLeftEncoderOffset = -0.4794921875; + private static final int kBackLeftDriveMotorId = 7; + private static final int kBackLeftSteerMotorId = 8; + private static final int kBackLeftEncoderId = 9; + private static final double kBackLeftEncoderOffset = 0.4814453125; + private static final boolean kBackLeftSteerInvert = true; - private static final double kBackLeftXPosInches = -10.5; - private static final double kBackLeftYPosInches = 10.5; + private static final double kBackLeftXPosInches = -10.375; + private static final double kBackLeftYPosInches = 10.375; // Back Right - private static final int kBackRightDriveMotorId = 3; - private static final int kBackRightSteerMotorId = 2; - private static final int kBackRightEncoderId = 1; - private static final double kBackRightEncoderOffset = -0.84130859375; + private static final int kBackRightDriveMotorId = 10; + private static final int kBackRightSteerMotorId = 11; + private static final int kBackRightEncoderId = 12; + private static final double kBackRightEncoderOffset = 0.300537109375; + private static final boolean kBackRightSteerInvert = true; - private static final double kBackRightXPosInches = -10.5; - private static final double kBackRightYPosInches = -10.5; + private static final double kBackRightXPosInches = -10.375; + private static final double kBackRightYPosInches = -10.375; private static final SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - Units.inchesToMeters(kFrontLeftXPosInches), - Units.inchesToMeters(kFrontLeftYPosInches), - kInvertLeftSide); + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + Units.inchesToMeters(kFrontLeftXPosInches), + Units.inchesToMeters(kFrontLeftYPosInches), + kInvertLeftSide) + .withSteerMotorInverted(kFrontLeftSteerInvert); private static final SwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - Units.inchesToMeters(kFrontRightXPosInches), - Units.inchesToMeters(kFrontRightYPosInches), - kInvertRightSide); + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + Units.inchesToMeters(kFrontRightXPosInches), + Units.inchesToMeters(kFrontRightYPosInches), + kInvertRightSide) + .withSteerMotorInverted(kFrontRightSteerInvert); private static final SwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - Units.inchesToMeters(kBackLeftXPosInches), - Units.inchesToMeters(kBackLeftYPosInches), - kInvertLeftSide); + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + Units.inchesToMeters(kBackLeftXPosInches), + Units.inchesToMeters(kBackLeftYPosInches), + kInvertLeftSide) + .withSteerMotorInverted(kBackLeftSteerInvert); private static final SwerveModuleConstants BackRight = ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - Units.inchesToMeters(kBackRightXPosInches), - Units.inchesToMeters(kBackRightYPosInches), - kInvertRightSide); + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + Units.inchesToMeters(kBackRightXPosInches), + Units.inchesToMeters(kBackRightYPosInches), + kInvertRightSide) + .withSteerMotorInverted(kBackRightSteerInvert); public static final SwerveSubsystem DriveTrain = new SwerveSubsystem(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 85870587..8c044857 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -68,10 +68,10 @@ public void periodic() { pigeonJerkVector = pigeonAccVector.minus(prevPigeonAccel).div(Constants.loopPeriodSecs); // Log everything to both AdvantageKit and SmartDashboard - Logger.recordOutput("Acceleration/Rio/Accel", rioAccVector); - Logger.recordOutput("Acceleration/Rio/Jerk", rioJerkVector); - Logger.recordOutput("Acceleration/Pigeon/Accel", pigeonAccVector); - Logger.recordOutput("Acceleration/Pigeon/Jerk", pigeonJerkVector); + Logger.recordOutput("Acceleration/Rio/Accel_mss", rioAccVector); + Logger.recordOutput("Acceleration/Rio/Jerk_msss", rioJerkVector); + Logger.recordOutput("Acceleration/Pigeon/Accel_mss", pigeonAccVector); + Logger.recordOutput("Acceleration/Pigeon/Jerk_msss", pigeonJerkVector); SmartDashboard.putNumber("RioXAccel", rioAccVector.getX()); SmartDashboard.putNumber("RioYAccel", rioAccVector.getY()); SmartDashboard.putNumber("PigeonXAccel", pigeonAccVector.getX()); From 6d401a9e31fd355fb707f8cdc1400afee1d3dde6 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 1 Nov 2024 17:27:44 -0700 Subject: [PATCH 38/57] Working on finding loop overruns modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java --- src/main/java/frc/robot/Robot.java | 10 ++++++---- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../robot/subsystems/accelerometer/Accelerometer.java | 6 ++++++ 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fdb1de8b..593377cd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,6 +19,7 @@ import frc.robot.Constants.PowerConstants; import frc.robot.RobotContainer.Ports; import frc.robot.util.VirtualSubsystem; +import java.util.List; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LogTable; import org.littletonrobotics.junction.LoggedRobot; @@ -38,6 +39,7 @@ public class Robot extends LoggedRobot { private Command m_autonomousCommand; private RobotContainer m_robotContainer; private Timer m_disabledTimer; + private List virtualSubsystems; /** * This function is run when the robot is first started up and should be used for any @@ -134,10 +136,10 @@ public void disabledInit() { @Override public void disabledPeriodic() { // After WHEEL_LOCK_TIME has elapsed, release the drive brakes - if (m_disabledTimer.hasElapsed(Constants.DrivebaseConstants.kWheelLockTime)) { - m_robotContainer.setMotorBrake(false); - m_disabledTimer.stop(); - } + // if (m_disabledTimer.hasElapsed(Constants.DrivebaseConstants.kWheelLockTime)) { + // m_robotContainer.setMotorBrake(false); + // m_disabledTimer.stop(); + // } } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d382403b..91e8645a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -234,8 +234,8 @@ public static class Ports { /* SUBSYSTEM CAN DEVICE IDS */ // This is where mechanism subsystem devices are defined // Example: - public static final CanDeviceId FLYWHEEL_LEADER = new CanDeviceId(3, ""); - public static final CanDeviceId FLYWHEEL_FOLLOWER = new CanDeviceId(4, ""); + public static final CanDeviceId FLYWHEEL_LEADER = new CanDeviceId(15, ""); + public static final CanDeviceId FLYWHEEL_FOLLOWER = new CanDeviceId(16, ""); /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */ // This is where digital I/O feedback devices are defined diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 8c044857..845ea6a5 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -46,6 +46,8 @@ public Accelerometer(Pigeon2 pigeonAccelerometer) { /** Get accelerations, compute jerks, log everything */ public void periodic() { + long start = System.nanoTime(); + // Compute the Rio's acceleration, rotated as needed in the XY plane (yaw) // RoboRio provides accelerations in `g`, from -8 to +8; convert to m/s^2 rioAccVector = @@ -80,5 +82,9 @@ public void periodic() { // Set the "previous" accelerations to the current for the next loop prevRioAccel = rioAccVector; prevPigeonAccel = pigeonAccVector; + + long finish = System.nanoTime(); + long timeElapsed = finish - start; + Logger.recordOutput("LoggedRobot/AccelCodeMS", (double) timeElapsed / 1.e6); } } From 8b343e26e37c4c94217314cd7afa70986690cf4b Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 6 Nov 2024 09:33:15 -0700 Subject: [PATCH 39/57] Update YAGSL vendordep new file: vendordeps/NavX.json new file: vendordeps/Phoenix5.json new file: vendordeps/ReduxLib_2024.json modified: vendordeps/yagsl.json --- vendordeps/NavX.json | 40 +++++++++ vendordeps/Phoenix5.json | 151 ++++++++++++++++++++++++++++++++++ vendordeps/ReduxLib_2024.json | 55 +++++++++++++ vendordeps/yagsl.json | 36 +++++++- 4 files changed, 280 insertions(+), 2 deletions(-) create mode 100644 vendordeps/NavX.json create mode 100644 vendordeps/Phoenix5.json create mode 100644 vendordeps/ReduxLib_2024.json diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json new file mode 100644 index 00000000..a142879f --- /dev/null +++ b/vendordeps/NavX.json @@ -0,0 +1,40 @@ +{ + "fileName": "NavX.json", + "name": "NavX", + "version": "2024.1.0", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2024", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2024/" + ], + "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-java", + "version": "2024.1.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-cpp", + "version": "2024.1.0", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 00000000..adf6e02a --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.33.1", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.33.1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.33.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.33.1", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.33.1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.33.1", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.33.1", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.1", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} diff --git a/vendordeps/ReduxLib_2024.json b/vendordeps/ReduxLib_2024.json new file mode 100644 index 00000000..a97c269e --- /dev/null +++ b/vendordeps/ReduxLib_2024.json @@ -0,0 +1,55 @@ +{ + "fileName": "ReduxLib_2024.json", + "name": "ReduxLib", + "version": "2024.3.2", + "frcYear": 2024, + "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", + "mavenUrls": [ + "https://maven.reduxrobotics.com/" + ], + "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2024.json", + "javaDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-java", + "version": "2024.3.2" + } + ], + "jniDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2024.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-cpp", + "version": "2024.3.2", + "libName": "ReduxLib-cpp", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} diff --git a/vendordeps/yagsl.json b/vendordeps/yagsl.json index d4cb6fed..9b441ee9 100644 --- a/vendordeps/yagsl.json +++ b/vendordeps/yagsl.json @@ -1,7 +1,7 @@ { "fileName": "yagsl.json", "name": "YAGSL", - "version": "2024.6.1.0", + "version": "2024.7.0", "frcYear": "2024", "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", "mavenUrls": [ @@ -12,7 +12,39 @@ { "groupId": "swervelib", "artifactId": "YAGSL-java", - "version": "2024.6.1.0" + "version": "2024.7.0" + } + ], + "requires": [ + { + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "errorMessage": "REVLib is required!", + "offlineFileName": "REVLib.json", + "onlineUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json" + }, + { + "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", + "errorMessage": "ReduxLib is required!", + "offlineFileName": "ReduxLib_2024.json", + "onlineUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2024.json" + }, + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix6 is required!", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + }, + { + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "errorMessage": "Phoenix5 is required!", + "offlineFileName": "Phoenix5.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json" + }, + { + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "errorMessage": "NavX is required!", + "offlineFileName": "NavX.json", + "onlineUrl": "https://dev.studica.com/releases/2024/NavX.json" } ], "jniDependencies": [], From 2756a5cafbe0a8fd1cc69917ee71205b85afafc4 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 6 Nov 2024 10:13:22 -0700 Subject: [PATCH 40/57] Include PDH Port number in RobotDeviceId Use the power port number for power monitoring. modified: .vscode/settings.json modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/util/PowerMonitoring.java renamed: src/main/java/frc/robot/util/CanDeviceId.java -> src/main/java/frc/robot/util/RobotDeviceId.java --- .vscode/settings.json | 1 + src/main/java/frc/robot/Constants.java | 19 ++++++++-- src/main/java/frc/robot/RobotContainer.java | 36 +++++++++---------- .../java/frc/robot/util/PowerMonitoring.java | 12 +++---- .../{CanDeviceId.java => RobotDeviceId.java} | 35 ++++++++++-------- 5 files changed, 62 insertions(+), 41 deletions(-) rename src/main/java/frc/robot/util/{CanDeviceId.java => RobotDeviceId.java} (54%) diff --git a/.vscode/settings.json b/.vscode/settings.json index 167548c9..f3fe2c03 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -30,6 +30,7 @@ ], "java.test.defaultConfig": "WPIlibUnitTests", "editor.indentSize": 2, + "editor.inlayHints.enabled": "on", "editor.tabSize": 2, "files.eol": "\n" } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 172ac917..e9bd5e11 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.RobotBase; +import frc.robot.RobotContainer.Ports; import frc.robot.generated.TunerConstants; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; @@ -161,12 +162,24 @@ public static final class PowerConstants { public static final double kSmallPortMaxCurrent = 20.; // The Power Distribution Module ports into which the DRIVE motors are plugged - public static final int[] kDrivePowerPorts = {0, 2, 4, 6}; + public static final int[] kDrivePowerPorts = { + Ports.FL_DRIVE.getPowerPort(), + Ports.FR_DRIVE.getPowerPort(), + Ports.BL_DRIVE.getPowerPort(), + Ports.BR_DRIVE.getPowerPort() + }; // The Power Distribution Module ports into which the STEER motors are plugged - public static final int[] kSteerPowerPorts = {1, 3, 5, 7}; + public static final int[] kSteerPowerPorts = { + Ports.FL_ROTATION.getPowerPort(), + Ports.FR_ROTATION.getPowerPort(), + Ports.BL_ROTATION.getPowerPort(), + Ports.BR_ROTATION.getPowerPort() + }; // Add additional subsystem port enumerations here for combined monitoring // Example: - // public static final int[] kElevatorPowerPorts = {9, 10}; + public static final int[] kFlywheelPowerPorts = { + Ports.FLYWHEEL_LEADER.getPowerPort(), Ports.FLYWHEEL_FOLLOWER.getPowerPort() + }; } /** Autonomous Action Constants ****************************************** */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 91e8645a..29e1e55d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,8 +40,8 @@ import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhoton; -import frc.robot.util.CanDeviceId; import frc.robot.util.OverrideSwitches; +import frc.robot.util.RobotDeviceId; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class RobotContainer { @@ -200,7 +200,7 @@ public void setMotorBrake(boolean brake) { m_drivebase.setMotorBrake(brake); } - /** List of Channel and CAN IDs ****************************************** */ + /** List of Device CAN and Power Distribution Circuit IDs **************** */ public static class Ports { /* DRIVETRAIN CAN DEVICE IDS */ @@ -210,32 +210,32 @@ public static class Ports { // // 0 1 // 2 3 - public static final CanDeviceId FL_DRIVE = new CanDeviceId(1, "canivore"); - public static final CanDeviceId FL_ROTATION = new CanDeviceId(2, "canivore"); - public static final CanDeviceId FL_CANCODER = new CanDeviceId(3, "canivore"); + public static final RobotDeviceId FL_DRIVE = new RobotDeviceId(1, "DriveTrain", 1); + public static final RobotDeviceId FL_ROTATION = new RobotDeviceId(2, "DriveTrain", 2); + public static final RobotDeviceId FL_CANCODER = new RobotDeviceId(3, "DriveTrain", null); - public static final CanDeviceId FR_DRIVE = new CanDeviceId(4, "canivore"); - public static final CanDeviceId FR_ROTATION = new CanDeviceId(5, "canivore"); - public static final CanDeviceId FR_CANCODER = new CanDeviceId(6, "canivore"); + public static final RobotDeviceId FR_DRIVE = new RobotDeviceId(4, "DriveTrain", 3); + public static final RobotDeviceId FR_ROTATION = new RobotDeviceId(5, "DriveTrain", 4); + public static final RobotDeviceId FR_CANCODER = new RobotDeviceId(6, "DriveTrain", null); - public static final CanDeviceId BL_DRIVE = new CanDeviceId(7, "canivore"); - public static final CanDeviceId BL_ROTATION = new CanDeviceId(8, "canivore"); - public static final CanDeviceId BL_CANCODER = new CanDeviceId(9, "canivore"); + public static final RobotDeviceId BL_DRIVE = new RobotDeviceId(7, "DriveTrain", 5); + public static final RobotDeviceId BL_ROTATION = new RobotDeviceId(8, "DriveTrain", 6); + public static final RobotDeviceId BL_CANCODER = new RobotDeviceId(9, "DriveTrain", null); - public static final CanDeviceId BR_DRIVE = new CanDeviceId(10, "canivore"); - public static final CanDeviceId BR_ROTATION = new CanDeviceId(11, "canivore"); - public static final CanDeviceId BR_CANCODER = new CanDeviceId(12, "canivore"); + public static final RobotDeviceId BR_DRIVE = new RobotDeviceId(10, "DriveTrain", 7); + public static final RobotDeviceId BR_ROTATION = new RobotDeviceId(11, "DriveTrain", 8); + public static final RobotDeviceId BR_CANCODER = new RobotDeviceId(12, "DriveTrain", null); - public static final CanDeviceId PIGEON = new CanDeviceId(13, "canivore"); + public static final RobotDeviceId PIGEON = new RobotDeviceId(13, "DriveTrain", null); /* POWER DISTRIBUTION CAN ID */ - public static final CanDeviceId POWER_CAN_DEVICE_ID = new CanDeviceId(1, ""); + public static final RobotDeviceId POWER_CAN_DEVICE_ID = new RobotDeviceId(1, null); /* SUBSYSTEM CAN DEVICE IDS */ // This is where mechanism subsystem devices are defined // Example: - public static final CanDeviceId FLYWHEEL_LEADER = new CanDeviceId(15, ""); - public static final CanDeviceId FLYWHEEL_FOLLOWER = new CanDeviceId(16, ""); + public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, 9); + public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, 10); /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */ // This is where digital I/O feedback devices are defined diff --git a/src/main/java/frc/robot/util/PowerMonitoring.java b/src/main/java/frc/robot/util/PowerMonitoring.java index 0cbf48eb..e69b32b4 100644 --- a/src/main/java/frc/robot/util/PowerMonitoring.java +++ b/src/main/java/frc/robot/util/PowerMonitoring.java @@ -54,10 +54,10 @@ public void periodic() { } // Add current monitoring by subsystem here // Example: - // double elevatorCurrent = 0.0; - // for (int port : PowerDistributionConstants.kElevatorPowerPorts) { - // elevatorCurrent += channelCurrents[port]; - // } + double flywheelCurrent = 0.0; + for (int port : PowerConstants.kFlywheelPowerPorts) { + flywheelCurrent += channelCurrents[port]; + } // Log values to AdvantageKit and to SmartDashboard Logger.recordOutput("PowerMonitor/TotalCurrent", totalCurrent); @@ -68,8 +68,8 @@ public void periodic() { SmartDashboard.putNumber("SteerCurrent", steerCurrent); // Add logging for subsystems here // Example: - // Logger.recordOutput("PowerMonitor/ElevatorCurrent", elevatorCurrent); - // SmartDashboard.putNumber("ElevatorCurrent", elevatorCurrent); + Logger.recordOutput("PowerMonitor/FlywheelCurrent", flywheelCurrent); + SmartDashboard.putNumber("FlywheelCurrent", flywheelCurrent); // Do something about setting priorities if drawing too much current diff --git a/src/main/java/frc/robot/util/CanDeviceId.java b/src/main/java/frc/robot/util/RobotDeviceId.java similarity index 54% rename from src/main/java/frc/robot/util/CanDeviceId.java rename to src/main/java/frc/robot/util/RobotDeviceId.java index 00b657cf..f5b77f81 100644 --- a/src/main/java/frc/robot/util/CanDeviceId.java +++ b/src/main/java/frc/robot/util/RobotDeviceId.java @@ -15,33 +15,40 @@ package frc.robot.util; -/** Class for wrapping CAN devices with a name and functionality */ -public class CanDeviceId { - private final int m_DeviceNumber; - private final String m_Bus; - - public CanDeviceId(int deviceNumber, String bus) { - m_DeviceNumber = deviceNumber; - m_Bus = bus; +/** Class for wrapping Robot / CAN devices with a name and functionality */ +public class RobotDeviceId { + private final int m_CANDeviceNumber; + private final String m_CANBus; + private final Integer m_PowerPort; + + public RobotDeviceId(int CANdeviceNumber, String CANbus, Integer powerPort) { + m_CANDeviceNumber = CANdeviceNumber; + m_CANBus = CANbus; + m_PowerPort = powerPort; } /** Use the default bus name (empty string) */ - public CanDeviceId(int deviceNumber) { - this(deviceNumber, ""); + public RobotDeviceId(int CANdeviceNumber, Integer powerPort) { + this(CANdeviceNumber, "", powerPort); } /** Get the CAN ID value for a named device */ public int getDeviceNumber() { - return m_DeviceNumber; + return m_CANDeviceNumber; } /** Get the CAN bus name for a named device */ public String getBus() { - return m_Bus; + return m_CANBus; + } + + /** Get the Power Port for a named device */ + public int getPowerPort() { + return m_PowerPort; } /** Check whether two named devices are, in fact, the same */ - public boolean equals(CanDeviceId other) { - return other.m_DeviceNumber == m_DeviceNumber && other.m_Bus == m_Bus; + public boolean equals(RobotDeviceId other) { + return other.m_CANDeviceNumber == m_CANDeviceNumber && other.m_CANBus == m_CANBus; } } From 224d48212cf14c75eb8c42517aca37a1cc27849a Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 6 Nov 2024 11:27:56 -0700 Subject: [PATCH 41/57] Add Advantake-Kit compliant Vision inputs modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java deleted: src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java new file: src/main/java/frc/robot/commands/swervedrive/auto/YAGSL_AutoBalanceCommand.java modified: src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDrive.java modified: src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java modified: src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteFieldDrive.java modified: src/main/java/frc/robot/generated/TunerConstants.java modified: src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java modified: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java modified: src/main/java/frc/robot/subsystems/swervedrive/yagsl_old/YAGSLSwerve.java modified: src/main/java/frc/robot/subsystems/vision/Vision.java modified: src/main/java/frc/robot/util/PowerMonitoring.java --- src/main/java/frc/robot/Constants.java | 37 +- src/main/java/frc/robot/RobotContainer.java | 10 +- .../swervedrive/auto/AutoBalanceCommand.java | 95 -- .../auto/YAGSL_AutoBalanceCommand.java | 102 ++ .../drivebase/YAGSL_AbsoluteDrive.java | 245 +-- .../drivebase/YAGSL_AbsoluteDriveAdv.java | 337 ++-- .../drivebase/YAGSL_AbsoluteFieldDrive.java | 183 +- .../frc/robot/generated/TunerConstants.java | 6 +- .../accelerometer/Accelerometer.java | 1 + .../swervedrive/SwerveSubsystem.java | 8 +- .../swervedrive/yagsl_old/YAGSLSwerve.java | 1507 +++++++++-------- .../frc/robot/subsystems/vision/Vision.java | 565 ++---- .../java/frc/robot/util/PowerMonitoring.java | 30 +- 13 files changed, 1413 insertions(+), 1713 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java create mode 100644 src/main/java/frc/robot/commands/swervedrive/auto/YAGSL_AutoBalanceCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e9bd5e11..5675a311 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,7 +28,6 @@ import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.RobotBase; -import frc.robot.RobotContainer.Ports; import frc.robot.generated.TunerConstants; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; @@ -150,7 +149,7 @@ public static final class PhysicalConstants { public static final double kLoopTime = 0.13; // s, 20ms + 110ms sprk max velocity lag } - /** Power Distribution Module Constants ********************************** */ + /** Power Distribution Constants ********************************** */ public static final class PowerConstants { // Set this to either kRev or kCTRE for the type of Power Distribution Module @@ -160,26 +159,6 @@ public static final class PowerConstants { public static final double kTotalMaxCurrent = 120.; public static final double kMotorPortMaxCurrent = 40.; public static final double kSmallPortMaxCurrent = 20.; - - // The Power Distribution Module ports into which the DRIVE motors are plugged - public static final int[] kDrivePowerPorts = { - Ports.FL_DRIVE.getPowerPort(), - Ports.FR_DRIVE.getPowerPort(), - Ports.BL_DRIVE.getPowerPort(), - Ports.BR_DRIVE.getPowerPort() - }; - // The Power Distribution Module ports into which the STEER motors are plugged - public static final int[] kSteerPowerPorts = { - Ports.FL_ROTATION.getPowerPort(), - Ports.FR_ROTATION.getPowerPort(), - Ports.BL_ROTATION.getPowerPort(), - Ports.BR_ROTATION.getPowerPort() - }; - // Add additional subsystem port enumerations here for combined monitoring - // Example: - public static final int[] kFlywheelPowerPorts = { - Ports.FLYWHEEL_LEADER.getPowerPort(), Ports.FLYWHEEL_FOLLOWER.getPowerPort() - }; } /** Autonomous Action Constants ****************************************** */ @@ -263,8 +242,20 @@ public static class OperatorConstants { public static final double kTurnConstant = 6; } + /** Vision Constants (Assuming PhotonVision) ***************************** */ + public static class VisionConstants { + + // AprilTag Identification Constants + public static final double kAmbiguityThreshold = 0.4; + public static final double kTargetLogTimeSecs = 0.1; + public static final double kFieldBorderMargin = 0.5; + public static final double kZMargin = 0.75; + public static final double kXYZStdDevCoefficient = 0.005; + public static final double kThetaStdDevCoefficient = 0.01; + } + /** AprilTag Field Layout ************************************************ */ - /* SEASON SPECIFIC! */ + /* SEASON SPECIFIC! -- This section is for 2024 (Crescendo) */ public static class AprilTagConstants { public static final double aprilTagWidth = Units.inchesToMeters(6.50); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 29e1e55d..3269512a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -249,14 +249,8 @@ public static class Ports { // public static final int INTAKE_SERVO = 0; } - /** Vision Constants and Camera Posses *********************************** */ - public static class VisionConstants { - public static final double ambiguityThreshold = 0.4; - public static final double targetLogTimeSecs = 0.1; - public static final double fieldBorderMargin = 0.5; - public static final double zMargin = 0.75; - public static final double xyStdDevCoefficient = 0.005; - public static final double thetaStdDevCoefficient = 0.01; + /** Vision Camera Posses ************************************************* */ + public static class Cameras { public static final Pose3d[] cameraPoses = switch (Constants.getRobot()) { diff --git a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java b/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java deleted file mode 100644 index 8baeed40..00000000 --- a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java +++ /dev/null @@ -1,95 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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. -// -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. -// -// NOTE: This module from the YAGSL Example Project - -package frc.robot.commands.swervedrive.auto; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; - -/** - * Auto Balance command using a simple PID controller. Created by Team 3512 ... - */ -public class AutoBalanceCommand extends Command { - - private final YAGSLSwerve swerveSubsystem; - private final PIDController controller; - - public AutoBalanceCommand(YAGSLSwerve swerveSubsystem) { - this.swerveSubsystem = swerveSubsystem; - controller = new PIDController(1.0, 0.0, 0.0); - controller.setTolerance(1); - controller.setSetpoint(0.0); - // each subsystem used by the command must be passed into the - // addRequirements() method (which takes a vararg of Subsystem) - addRequirements(this.swerveSubsystem); - } - - /** The initial subroutine of a command. Called once when the command is initially scheduled. */ - @Override - public void initialize() {} - - /** - * The main body of a command. Called repeatedly while the command is scheduled. (That is, it is - * called repeatedly until {@link #isFinished()}) returns true.) - */ - @Override - public void execute() { - SmartDashboard.putBoolean("At Tolerance", controller.atSetpoint()); - - double translationVal = - MathUtil.clamp( - controller.calculate(swerveSubsystem.getPitch().getDegrees(), 0.0), -0.5, 0.5); - swerveSubsystem.drive(new Translation2d(translationVal, 0.0), 0.0, true); - } - - /** - * Returns whether this command has finished. Once a command finishes -- indicated by this method - * returning true -- the scheduler will call its {@link #end(boolean)} method. - * - *

Returning false will result in the command never ending automatically. It may still be - * cancelled manually or interrupted by another command. Hard coding this command to always return - * true will result in the command executing once and finishing immediately. It is recommended to - * use * {@link edu.wpi.first.wpilibj2.command.InstantCommand InstantCommand} for such an - * operation. - * - * @return whether this command has finished. - */ - @Override - public boolean isFinished() { - return controller.atSetpoint(); - } - - /** - * The action to take when the command ends. Called when either the command finishes normally -- - * that is it is called when {@link #isFinished()} returns true -- or when it is - * interrupted/canceled. This is where you may want to wrap up loose ends, like shutting off a - * motor that was being used in the command. - * - * @param interrupted whether the command was interrupted/canceled - */ - @Override - public void end(boolean interrupted) { - swerveSubsystem.lock(); - } -} diff --git a/src/main/java/frc/robot/commands/swervedrive/auto/YAGSL_AutoBalanceCommand.java b/src/main/java/frc/robot/commands/swervedrive/auto/YAGSL_AutoBalanceCommand.java new file mode 100644 index 00000000..fa2f309c --- /dev/null +++ b/src/main/java/frc/robot/commands/swervedrive/auto/YAGSL_AutoBalanceCommand.java @@ -0,0 +1,102 @@ +// // Copyright (c) 2024 Az-FIRST +// // http://github.com/AZ-First +// // +// // This program is free software; you can redistribute it and/or +// // modify it under the terms of the GNU General Public License +// // version 3 as published by the Free Software Foundation or +// // available in the root directory of this project. +// // +// // 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. +// // +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. +// // +// // NOTE: This module from the YAGSL Example Project + +// package frc.robot.commands.swervedrive.auto; + +// import edu.wpi.first.math.MathUtil; +// import edu.wpi.first.math.controller.PIDController; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.Command; +// import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; + +// /** +// * Auto Balance command using a simple PID controller. Created by Team 3512 ... +// */ +// public class YAGSL_AutoBalanceCommand extends Command { + +// private final YAGSLSwerve swerveSubsystem; +// private final PIDController controller; + +// public YAGSL_AutoBalanceCommand(YAGSLSwerve swerveSubsystem) { +// this.swerveSubsystem = swerveSubsystem; +// controller = new PIDController(1.0, 0.0, 0.0); +// controller.setTolerance(1); +// controller.setSetpoint(0.0); +// // each subsystem used by the command must be passed into the +// // addRequirements() method (which takes a vararg of Subsystem) +// addRequirements(this.swerveSubsystem); +// } + +// /** The initial subroutine of a command. Called once when the command is initially scheduled. +// */ +// @Override +// public void initialize() {} + +// /** +// * The main body of a command. Called repeatedly while the command is scheduled. (That is, it +// is +// * called repeatedly until {@link #isFinished()}) returns true.) +// */ +// @Override +// public void execute() { +// SmartDashboard.putBoolean("At Tolerance", controller.atSetpoint()); + +// double translationVal = +// MathUtil.clamp( +// controller.calculate(swerveSubsystem.getPitch().getDegrees(), 0.0), -0.5, 0.5); +// swerveSubsystem.drive(new Translation2d(translationVal, 0.0), 0.0, true); +// } + +// /** +// * Returns whether this command has finished. Once a command finishes -- indicated by this +// method +// * returning true -- the scheduler will call its {@link #end(boolean)} method. +// * +// *

Returning false will result in the command never ending automatically. It may still be +// * cancelled manually or interrupted by another command. Hard coding this command to always +// return +// * true will result in the command executing once and finishing immediately. It is recommended +// to +// * use * {@link edu.wpi.first.wpilibj2.command.InstantCommand InstantCommand} for such an +// * operation. +// * +// * @return whether this command has finished. +// */ +// @Override +// public boolean isFinished() { +// return controller.atSetpoint(); +// } + +// /** +// * The action to take when the command ends. Called when either the command finishes normally +// -- +// * that is it is called when {@link #isFinished()} returns true -- or when it is +// * interrupted/canceled. This is where you may want to wrap up loose ends, like shutting off a +// * motor that was being used in the command. +// * +// * @param interrupted whether the command was interrupted/canceled +// */ +// @Override +// public void end(boolean interrupted) { +// swerveSubsystem.lock(); +// } +// } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDrive.java index 727d958f..da035d86 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDrive.java @@ -1,135 +1,140 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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. -// -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. -// -// NOTE: This module from the YAGSL Example Project +// // Copyright (c) 2024 Az-FIRST +// // http://github.com/AZ-First +// // +// // This program is free software; you can redistribute it and/or +// // modify it under the terms of the GNU General Public License +// // version 3 as published by the Free Software Foundation or +// // available in the root directory of this project. +// // +// // 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. +// // +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. +// // +// // NOTE: This module from the YAGSL Example Project -package frc.robot.commands.swervedrive.drivebase; +// package frc.robot.commands.swervedrive.drivebase; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.PhysicalConstants; -import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; -import java.util.List; -import java.util.function.DoubleSupplier; -import swervelib.SwerveController; -import swervelib.math.SwerveMath; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.kinematics.ChassisSpeeds; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.Command; +// import frc.robot.Constants.PhysicalConstants; +// import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; +// import java.util.List; +// import java.util.function.DoubleSupplier; +// import swervelib.SwerveController; +// import swervelib.math.SwerveMath; -/** An example command that uses an example subsystem. */ -public class YAGSL_AbsoluteDrive extends Command { +// /** An example command that uses an example subsystem. */ +// public class YAGSL_AbsoluteDrive extends Command { - private final YAGSLSwerve swerve; - private final DoubleSupplier vX, vY; - private final DoubleSupplier headingHorizontal, headingVertical; - private boolean initRotation = false; +// private final YAGSLSwerve swerve; +// private final DoubleSupplier vX, vY; +// private final DoubleSupplier headingHorizontal, headingVertical; +// private boolean initRotation = false; - /** - * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, - * where x is torwards/away from alliance wall and y is left/right. headingHorzontal and - * headingVertical are the Cartesian coordinates from which the robot's angle will be derived— - * they will be converted to a polar angle, which the robot will rotate to. - * - * @param swerve The swerve drivebase subsystem. - * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range - * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. - * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range - * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when - * looking through the driver station glass. - * @param headingHorizontal DoubleSupplier that supplies the horizontal component of the robot's - * heading angle. In the robot coordinate system, this is along the same axis as vY. Should - * range from -1 to 1 with no deadband. Positive is towards the left wall when looking through - * the driver station glass. - * @param headingVertical DoubleSupplier that supplies the vertical component of the robot's - * heading angle. In the robot coordinate system, this is along the same axis as vX. Should - * range from -1 to 1 with no deadband. Positive is away from the alliance wall. - */ - public YAGSL_AbsoluteDrive( - YAGSLSwerve swerve, - DoubleSupplier vX, - DoubleSupplier vY, - DoubleSupplier headingHorizontal, - DoubleSupplier headingVertical) { - this.swerve = swerve; - this.vX = vX; - this.vY = vY; - this.headingHorizontal = headingHorizontal; - this.headingVertical = headingVertical; +// /** +// * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation +// inputs, +// * where x is torwards/away from alliance wall and y is left/right. headingHorzontal and +// * headingVertical are the Cartesian coordinates from which the robot's angle will be derived— +// * they will be converted to a polar angle, which the robot will rotate to. +// * +// * @param swerve The swerve drivebase subsystem. +// * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the +// range +// * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. +// * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the +// range +// * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when +// * looking through the driver station glass. +// * @param headingHorizontal DoubleSupplier that supplies the horizontal component of the +// robot's +// * heading angle. In the robot coordinate system, this is along the same axis as vY. Should +// * range from -1 to 1 with no deadband. Positive is towards the left wall when looking +// through +// * the driver station glass. +// * @param headingVertical DoubleSupplier that supplies the vertical component of the robot's +// * heading angle. In the robot coordinate system, this is along the same axis as vX. Should +// * range from -1 to 1 with no deadband. Positive is away from the alliance wall. +// */ +// public YAGSL_AbsoluteDrive( +// YAGSLSwerve swerve, +// DoubleSupplier vX, +// DoubleSupplier vY, +// DoubleSupplier headingHorizontal, +// DoubleSupplier headingVertical) { +// this.swerve = swerve; +// this.vX = vX; +// this.vY = vY; +// this.headingHorizontal = headingHorizontal; +// this.headingVertical = headingVertical; - addRequirements(swerve); - } +// addRequirements(swerve); +// } - @Override - public void initialize() { - initRotation = true; - } +// @Override +// public void initialize() { +// initRotation = true; +// } - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { +// // Called every time the scheduler runs while the command is scheduled. +// @Override +// public void execute() { - // Get the desired chassis speeds based on a 2 joystick module. - ChassisSpeeds desiredSpeeds = - swerve.getTargetSpeeds( - vX.getAsDouble(), - vY.getAsDouble(), - headingHorizontal.getAsDouble(), - headingVertical.getAsDouble()); +// // Get the desired chassis speeds based on a 2 joystick module. +// ChassisSpeeds desiredSpeeds = +// swerve.getTargetSpeeds( +// vX.getAsDouble(), +// vY.getAsDouble(), +// headingHorizontal.getAsDouble(), +// headingVertical.getAsDouble()); - // Prevent Movement After Auto - if (initRotation) { - if (headingHorizontal.getAsDouble() == 0 && headingVertical.getAsDouble() == 0) { - // Get the curretHeading - Rotation2d firstLoopHeading = swerve.getHeading(); +// // Prevent Movement After Auto +// if (initRotation) { +// if (headingHorizontal.getAsDouble() == 0 && headingVertical.getAsDouble() == 0) { +// // Get the curretHeading +// Rotation2d firstLoopHeading = swerve.getHeading(); - // Set the Current Heading to the desired Heading - desiredSpeeds = - swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos()); - } - // Dont Init Rotation Again - initRotation = false; - } +// // Set the Current Heading to the desired Heading +// desiredSpeeds = +// swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos()); +// } +// // Dont Init Rotation Again +// initRotation = false; +// } - // Limit velocity to prevent tippy - Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); - translation = - SwerveMath.limitVelocity( - translation, - swerve.getFieldVelocity(), - swerve.getPose(), - PhysicalConstants.kLoopTime, - PhysicalConstants.kRobotMass, - List.of(PhysicalConstants.kChassis), - swerve.getSwerveDriveConfiguration()); - SmartDashboard.putNumber("LimitedTranslation", translation.getX()); - SmartDashboard.putString("Translation", translation.toString()); +// // Limit velocity to prevent tippy +// Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); +// translation = +// SwerveMath.limitVelocity( +// translation, +// swerve.getFieldVelocity(), +// swerve.getPose(), +// PhysicalConstants.kLoopTime, +// PhysicalConstants.kRobotMass, +// List.of(PhysicalConstants.kChassis), +// swerve.getSwerveDriveConfiguration()); +// SmartDashboard.putNumber("LimitedTranslation", translation.getX()); +// SmartDashboard.putString("Translation", translation.toString()); - // Make the robot move - swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); - } +// // Make the robot move +// swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); +// } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} +// // Called once the command ends or is interrupted. +// @Override +// public void end(boolean interrupted) {} - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} +// // Returns true when the command should end. +// @Override +// public boolean isFinished() { +// return false; +// } +// } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java index c787a547..d959948f 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java @@ -1,166 +1,171 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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. -// -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. -// -// NOTE: This module from the YAGSL Example Project - -package frc.robot.commands.swervedrive.drivebase; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.OperatorConstants; -import frc.robot.Constants.PhysicalConstants; -import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; -import java.util.List; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; -import swervelib.SwerveController; -import swervelib.math.SwerveMath; - -/** A more advanced Swerve Control System that has 4 buttons for which direction to face */ -public class YAGSL_AbsoluteDriveAdv extends Command { - - private final YAGSLSwerve swerve; - private final DoubleSupplier vX, vY; - private final DoubleSupplier headingAdjust; - private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; - private boolean resetHeading = false; - - /** - * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, - * where x is torwards/away from alliance wall and y is left/right. Heading Adjust changes the - * current heading after being multipied by a constant. The look booleans are shortcuts to get the - * robot to face a certian direction. Based off of ideas in - * https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172 - * - * @param swerve The swerve drivebase subsystem. - * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range - * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. - * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range - * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when - * looking through the driver station glass. - * @param headingAdjust DoubleSupplier that supplies the component of the robot's heading angle - * that should be adjusted. Should range from -1 to 1 with deadband already accounted for. - * @param lookAway Face the robot towards the opposing alliance's wall in the same direction the - * driver is facing - * @param lookTowards Face the robot towards the driver - * @param lookLeft Face the robot left - * @param lookRight Face the robot right - */ - public YAGSL_AbsoluteDriveAdv( - YAGSLSwerve swerve, - DoubleSupplier vX, - DoubleSupplier vY, - DoubleSupplier headingAdjust, - BooleanSupplier lookAway, - BooleanSupplier lookTowards, - BooleanSupplier lookLeft, - BooleanSupplier lookRight) { - this.swerve = swerve; - this.vX = vX; - this.vY = vY; - this.headingAdjust = headingAdjust; - this.lookAway = lookAway; - this.lookTowards = lookTowards; - this.lookLeft = lookLeft; - this.lookRight = lookRight; - - addRequirements(swerve); - } - - @Override - public void initialize() { - resetHeading = true; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - double headingX = 0; - double headingY = 0; - - // These are written to allow combinations for 45 angles - // Face Away from Drivers - if (lookAway.getAsBoolean()) { - headingY = -1; - } - // Face Right - if (lookRight.getAsBoolean()) { - headingX = 1; - } - // Face Left - if (lookLeft.getAsBoolean()) { - headingX = -1; - } - // Face Towards the Drivers - if (lookTowards.getAsBoolean()) { - headingY = 1; - } - - // Prevent Movement After Auto - if (resetHeading) { - if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) == 0) { - // Get the curret Heading - Rotation2d currentHeading = swerve.getHeading(); - - // Set the Current Heading to the desired Heading - headingX = currentHeading.getSin(); - headingY = currentHeading.getCos(); - } - // Dont reset Heading Again - resetHeading = false; - } - - ChassisSpeeds desiredSpeeds = - swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY); - - // Limit velocity to prevent tippy - Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); - translation = - SwerveMath.limitVelocity( - translation, - swerve.getFieldVelocity(), - swerve.getPose(), - PhysicalConstants.kLoopTime, - PhysicalConstants.kRobotMass, - List.of(PhysicalConstants.kChassis), - swerve.getSwerveDriveConfiguration()); - SmartDashboard.putNumber("LimitedTranslation", translation.getX()); - SmartDashboard.putString("Translation", translation.toString()); - - // Make the robot move - if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { - resetHeading = true; - swerve.drive( - translation, (OperatorConstants.kTurnConstant * -headingAdjust.getAsDouble()), true); - } else { - swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} +// // Copyright (c) 2024 Az-FIRST +// // http://github.com/AZ-First +// // +// // This program is free software; you can redistribute it and/or +// // modify it under the terms of the GNU General Public License +// // version 3 as published by the Free Software Foundation or +// // available in the root directory of this project. +// // +// // 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. +// // +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. +// // +// // NOTE: This module from the YAGSL Example Project + +// package frc.robot.commands.swervedrive.drivebase; + +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.kinematics.ChassisSpeeds; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.Command; +// import frc.robot.Constants.OperatorConstants; +// import frc.robot.Constants.PhysicalConstants; +// import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; +// import java.util.List; +// import java.util.function.BooleanSupplier; +// import java.util.function.DoubleSupplier; +// import swervelib.SwerveController; +// import swervelib.math.SwerveMath; + +// /** A more advanced Swerve Control System that has 4 buttons for which direction to face */ +// public class YAGSL_AbsoluteDriveAdv extends Command { + +// private final YAGSLSwerve swerve; +// private final DoubleSupplier vX, vY; +// private final DoubleSupplier headingAdjust; +// private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; +// private boolean resetHeading = false; + +// /** +// * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation +// inputs, +// * where x is torwards/away from alliance wall and y is left/right. Heading Adjust changes the +// * current heading after being multipied by a constant. The look booleans are shortcuts to get +// the +// * robot to face a certian direction. Based off of ideas in +// * https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172 +// * +// * @param swerve The swerve drivebase subsystem. +// * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the +// range +// * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. +// * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the +// range +// * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when +// * looking through the driver station glass. +// * @param headingAdjust DoubleSupplier that supplies the component of the robot's heading angle +// * that should be adjusted. Should range from -1 to 1 with deadband already accounted for. +// * @param lookAway Face the robot towards the opposing alliance's wall in the same direction +// the +// * driver is facing +// * @param lookTowards Face the robot towards the driver +// * @param lookLeft Face the robot left +// * @param lookRight Face the robot right +// */ +// public YAGSL_AbsoluteDriveAdv( +// YAGSLSwerve swerve, +// DoubleSupplier vX, +// DoubleSupplier vY, +// DoubleSupplier headingAdjust, +// BooleanSupplier lookAway, +// BooleanSupplier lookTowards, +// BooleanSupplier lookLeft, +// BooleanSupplier lookRight) { +// this.swerve = swerve; +// this.vX = vX; +// this.vY = vY; +// this.headingAdjust = headingAdjust; +// this.lookAway = lookAway; +// this.lookTowards = lookTowards; +// this.lookLeft = lookLeft; +// this.lookRight = lookRight; + +// addRequirements(swerve); +// } + +// @Override +// public void initialize() { +// resetHeading = true; +// } + +// // Called every time the scheduler runs while the command is scheduled. +// @Override +// public void execute() { +// double headingX = 0; +// double headingY = 0; + +// // These are written to allow combinations for 45 angles +// // Face Away from Drivers +// if (lookAway.getAsBoolean()) { +// headingY = -1; +// } +// // Face Right +// if (lookRight.getAsBoolean()) { +// headingX = 1; +// } +// // Face Left +// if (lookLeft.getAsBoolean()) { +// headingX = -1; +// } +// // Face Towards the Drivers +// if (lookTowards.getAsBoolean()) { +// headingY = 1; +// } + +// // Prevent Movement After Auto +// if (resetHeading) { +// if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) == 0) { +// // Get the curret Heading +// Rotation2d currentHeading = swerve.getHeading(); + +// // Set the Current Heading to the desired Heading +// headingX = currentHeading.getSin(); +// headingY = currentHeading.getCos(); +// } +// // Dont reset Heading Again +// resetHeading = false; +// } + +// ChassisSpeeds desiredSpeeds = +// swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY); + +// // Limit velocity to prevent tippy +// Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); +// translation = +// SwerveMath.limitVelocity( +// translation, +// swerve.getFieldVelocity(), +// swerve.getPose(), +// PhysicalConstants.kLoopTime, +// PhysicalConstants.kRobotMass, +// List.of(PhysicalConstants.kChassis), +// swerve.getSwerveDriveConfiguration()); +// SmartDashboard.putNumber("LimitedTranslation", translation.getX()); +// SmartDashboard.putString("Translation", translation.toString()); + +// // Make the robot move +// if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { +// resetHeading = true; +// swerve.drive( +// translation, (OperatorConstants.kTurnConstant * -headingAdjust.getAsDouble()), true); +// } else { +// swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); +// } +// } + +// // Called once the command ends or is interrupted. +// @Override +// public void end(boolean interrupted) {} + +// // Returns true when the command should end. +// @Override +// public boolean isFinished() { +// return false; +// } +// } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteFieldDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteFieldDrive.java index 726c37a9..2fff696f 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteFieldDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteFieldDrive.java @@ -1,104 +1,107 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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. -// -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. -// -// NOTE: This module from the YAGSL Example Project +// // Copyright (c) 2024 Az-FIRST +// // http://github.com/AZ-First +// // +// // This program is free software; you can redistribute it and/or +// // modify it under the terms of the GNU General Public License +// // version 3 as published by the Free Software Foundation or +// // available in the root directory of this project. +// // +// // 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. +// // +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. +// // +// // NOTE: This module from the YAGSL Example Project -package frc.robot.commands.swervedrive.drivebase; +// package frc.robot.commands.swervedrive.drivebase; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.PhysicalConstants; -import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; -import java.util.List; -import java.util.function.DoubleSupplier; -import swervelib.SwerveController; -import swervelib.math.SwerveMath; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.kinematics.ChassisSpeeds; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.Command; +// import frc.robot.Constants.PhysicalConstants; +// import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; +// import java.util.List; +// import java.util.function.DoubleSupplier; +// import swervelib.SwerveController; +// import swervelib.math.SwerveMath; -/** An example command that uses an example subsystem. */ -public class YAGSL_AbsoluteFieldDrive extends Command { +// /** An example command that uses an example subsystem. */ +// public class YAGSL_AbsoluteFieldDrive extends Command { - private final YAGSLSwerve swerve; - private final DoubleSupplier vX, vY, heading; +// private final YAGSLSwerve swerve; +// private final DoubleSupplier vX, vY, heading; - /** - * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, - * where x is torwards/away from alliance wall and y is left/right. headingHorzontal and - * headingVertical are the Cartesian coordinates from which the robot's angle will be derived— - * they will be converted to a polar angle, which the robot will rotate to. - * - * @param swerve The swerve drivebase subsystem. - * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range - * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. - * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range - * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when - * looking through the driver station glass. - * @param heading DoubleSupplier that supplies the robot's heading angle. - */ - public YAGSL_AbsoluteFieldDrive( - YAGSLSwerve swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier heading) { - this.swerve = swerve; - this.vX = vX; - this.vY = vY; - this.heading = heading; +// /** +// * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation +// inputs, +// * where x is torwards/away from alliance wall and y is left/right. headingHorzontal and +// * headingVertical are the Cartesian coordinates from which the robot's angle will be derived— +// * they will be converted to a polar angle, which the robot will rotate to. +// * +// * @param swerve The swerve drivebase subsystem. +// * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the +// range +// * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. +// * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the +// range +// * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when +// * looking through the driver station glass. +// * @param heading DoubleSupplier that supplies the robot's heading angle. +// */ +// public YAGSL_AbsoluteFieldDrive( +// YAGSLSwerve swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier heading) { +// this.swerve = swerve; +// this.vX = vX; +// this.vY = vY; +// this.heading = heading; - addRequirements(swerve); - } +// addRequirements(swerve); +// } - @Override - public void initialize() {} +// @Override +// public void initialize() {} - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { +// // Called every time the scheduler runs while the command is scheduled. +// @Override +// public void execute() { - // Get the desired chassis speeds based on a 2 joystick module. +// // Get the desired chassis speeds based on a 2 joystick module. - ChassisSpeeds desiredSpeeds = - swerve.getTargetSpeeds( - vX.getAsDouble(), vY.getAsDouble(), new Rotation2d(heading.getAsDouble() * Math.PI)); +// ChassisSpeeds desiredSpeeds = +// swerve.getTargetSpeeds( +// vX.getAsDouble(), vY.getAsDouble(), new Rotation2d(heading.getAsDouble() * Math.PI)); - // Limit velocity to prevent tippy - Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); - translation = - SwerveMath.limitVelocity( - translation, - swerve.getFieldVelocity(), - swerve.getPose(), - PhysicalConstants.kLoopTime, - PhysicalConstants.kRobotMass, - List.of(PhysicalConstants.kChassis), - swerve.getSwerveDriveConfiguration()); - SmartDashboard.putNumber("LimitedTranslation", translation.getX()); - SmartDashboard.putString("Translation", translation.toString()); +// // Limit velocity to prevent tippy +// Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); +// translation = +// SwerveMath.limitVelocity( +// translation, +// swerve.getFieldVelocity(), +// swerve.getPose(), +// PhysicalConstants.kLoopTime, +// PhysicalConstants.kRobotMass, +// List.of(PhysicalConstants.kChassis), +// swerve.getSwerveDriveConfiguration()); +// SmartDashboard.putNumber("LimitedTranslation", translation.getX()); +// SmartDashboard.putString("Translation", translation.toString()); - // Make the robot move - swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); - } +// // Make the robot move +// swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); +// } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} +// // Called once the command ends or is interrupted. +// @Override +// public void end(boolean interrupted) {} - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} +// // Returns true when the command should end. +// @Override +// public boolean isFinished() { +// return false; +// } +// } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index b68a658e..c48599ec 100755 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -29,10 +29,12 @@ public class TunerConstants { // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.Voltage; + private static final ClosedLoopOutputType steerClosedLoopOutput = + ClosedLoopOutputType.TorqueCurrentFOC; // The closed-loop output type to use for the drive motors; // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; + private static final ClosedLoopOutputType driveClosedLoopOutput = + ClosedLoopOutputType.TorqueCurrentFOC; // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 845ea6a5..aa281e18 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -83,6 +83,7 @@ public void periodic() { prevRioAccel = rioAccVector; prevPigeonAccel = pigeonAccVector; + // Quick logging to see how long this periodic takes long finish = System.nanoTime(); long timeElapsed = finish - start; Logger.recordOutput("LoggedRobot/AccelCodeMS", (double) timeElapsed / 1.e6); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 3a9bebb5..79decf81 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -245,10 +245,10 @@ public void periodic() { }); } - // When vision is enabled we must manually update odometry in SwerveDrive - if (visionDriveTest) { - vision.updatePoseEstimation(this); - } + // // When vision is enabled we must manually update odometry in SwerveDrive + // if (visionDriveTest) { + // vision.updatePoseEstimation(this); + // } /** Log Telemetry Data to AdvantageKit */ Logger.recordOutput("SwerveDrive/Comment", "This is a comment!"); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/yagsl_old/YAGSLSwerve.java b/src/main/java/frc/robot/subsystems/swervedrive/yagsl_old/YAGSLSwerve.java index 7efbee0c..187c601a 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/yagsl_old/YAGSLSwerve.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/yagsl_old/YAGSLSwerve.java @@ -1,741 +1,766 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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. -// -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. -// -// NOTE: This module based on the YAGSL Example Project - -package frc.robot.subsystems.swervedrive.yagsl_old; - -import static frc.robot.Constants.DrivebaseConstants.*; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.ReplanningConfig; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import frc.robot.Constants.AutonConstants; -import frc.robot.subsystems.vision.Vision; -import java.io.File; -import java.util.Arrays; -import java.util.function.DoubleSupplier; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; -import org.photonvision.PhotonCamera; -import org.photonvision.targeting.PhotonPipelineResult; -import swervelib.SwerveController; -import swervelib.SwerveDrive; -import swervelib.SwerveDriveTest; -import swervelib.math.SwerveMath; -import swervelib.parser.SwerveControllerConfiguration; -import swervelib.parser.SwerveDriveConfiguration; -import swervelib.parser.SwerveParser; -import swervelib.telemetry.SwerveDriveTelemetry; -import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; - -/** YAGSLSwerve module */ -public class YAGSLSwerve extends SubsystemBase { - - /** Swerve drive object */ - private final SwerveDrive swerveDrive; - - /** PhotonVision class to keep an accurate odometry */ - private Vision vision; - - /** Enable vision odometry updates while driving */ - private final boolean visionDriveTest = false; - - /** - * Initialize {@link SwerveDrive} with the directory provided. - * - * @param directory Directory of swerve drive config files. - */ - public YAGSLSwerve(File directory) { - - // Angle conversion factor is 360 / (GEAR RATIO * ENCODER RESOLUTION) - double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(kTurnGearRatio); - - // Motor conversion factor is (PI * WHEEL DIAMETER IN METERS) / (GEAR RATIO * ENCODER - // RESOLUTION) - double driveConversionFactor = - SwerveMath.calculateMetersPerRotation(kWheelRadius * 2.0, kDriveGearRatio); - - // Output to console and to AdvantageKit - System.out.println("\"conversionFactors\": {"); - System.out.println("\t\"angle\": {\"factor\": " + angleConversionFactor + " },"); - System.out.println("\t\"drive\": {\"factor\": " + driveConversionFactor + " }"); - System.out.println("}"); - Logger.recordOutput("SwerveDive/ConversionFactors/Angle", angleConversionFactor); - Logger.recordOutput("SwerveDive/ConversionFactors/Drive", driveConversionFactor); - - // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being - // created. - SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; - try { - swerveDrive = new SwerveParser(directory).createSwerveDrive(kMaxLinearSpeed); - // Alternative method if you don't want to supply the conversion factor via JSON files. - // swerveDrive = new SwerveParser_RBSI(directory).createSwerveDrive(maximumSpeed, - // angleConversionFactor, driveConversionFactor); - } catch (Exception e) { - throw new RuntimeException(e); - } - // Heading correction should only be used while controlling the robot via angle. - swerveDrive.setHeadingCorrection(false); - // Disables cosine compensation for simulations since it causes discrepancies not seen in real - // life. - if (SwerveDriveTelemetry.isSimulation) { - swerveDrive.setCosineCompensator(false); - } else { - swerveDrive.setCosineCompensator(true); - } - - // Correct for skew that gets worse as angular velocity increases. Start with a coefficient of - // 0.1 - swerveDrive.setAngularVelocityCompensation(true, true, 0.1); - // Enable if you want to resynchronize your absolute encoders and motor encoders periodically - // when they are not moving. - swerveDrive.setModuleEncoderAutoSynchronize(false, 1); - // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. - // Throws warning if not possible - swerveDrive.pushOffsetsToEncoders(); - - if (visionDriveTest) { - setupPhotonVision(); - // Stop the odometry thread if we are using vision that way we can synchronize updates better. - swerveDrive.stopOdometryThread(); - } - setupPathPlanner(); - } - - /** - * Construct the swerve drive. - * - * @param driveCfg SwerveDriveConfiguration for the swerve. - * @param controllerCfg Swerve Controller. - */ - public YAGSLSwerve( - SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { - swerveDrive = new SwerveDrive(driveCfg, controllerCfg, kMaxLinearSpeed); - } - - /** Setup the photon vision class. */ - public void setupPhotonVision() { - vision = new Vision(swerveDrive::getPose, swerveDrive.field); - } - - /** Setup AutoBuilder for PathPlanner. */ - public void setupPathPlanner() { - AutoBuilder.configureHolonomic( - this::getPose, // Robot pose supplier - this::resetOdometry, // Method to reset odometry (called if your auto has a starting pose) - this::getRobotVelocity, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - this::setChassisSpeeds, // Driver method given ROBOT RELATIVE ChassisSpeeds - new HolonomicPathFollowerConfig( - AutonConstants.kAutoTranslatePID, - AutonConstants.kAutoAnglePID, - kMaxLinearSpeed, - kDriveBaseRadius, - new ReplanningConfig()), - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - var alliance = DriverStation.getAlliance(); - return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; - }, - this // Reference to this subsystem to set requirements - ); - } - - /** Periodic function -- update odometry and log everything */ - @Override - public void periodic() { - // // When vision is enabled we must manually update odometry in SwerveDrive - // if (visionDriveTest) { - // swerveDrive.updateOdometry(); - // vision.updatePoseEstimation(swerveDrive); - // } - - /** Log Telemetry Data to AdvantageKit */ - Logger.recordOutput("SwerveDive/Telemetry/moduleCount", SwerveDriveTelemetry.moduleCount); - Logger.recordOutput("SwerveDive/Telemetry/wheelLocations", SwerveDriveTelemetry.wheelLocations); - Logger.recordOutput("SwerveDive/Telemetry/measuredStates", SwerveDriveTelemetry.measuredStates); - Logger.recordOutput("SwerveDive/Telemetry/desiredStates", SwerveDriveTelemetry.desiredStates); - Logger.recordOutput("SwerveDive/Telemetry/robotRotation", SwerveDriveTelemetry.robotRotation); - Logger.recordOutput("SwerveDive/Telemetry/maxSpeed", SwerveDriveTelemetry.maxSpeed); - Logger.recordOutput("SwerveDive/Telemetry/rotationUnit", SwerveDriveTelemetry.rotationUnit); - Logger.recordOutput("SwerveDive/Telemetry/sizeLeftRight", SwerveDriveTelemetry.sizeLeftRight); - Logger.recordOutput("SwerveDive/Telemetry/sizeFrontBack", SwerveDriveTelemetry.sizeFrontBack); - Logger.recordOutput( - "SwerveDive/Telemetry/forwardDirection", SwerveDriveTelemetry.forwardDirection); - Logger.recordOutput( - "SwerveDive/Telemetry/maxAngularVelocity", SwerveDriveTelemetry.maxAngularVelocity); - Logger.recordOutput( - "SwerveDive/Telemetry/measuredChassisSpeeds", SwerveDriveTelemetry.measuredChassisSpeeds); - Logger.recordOutput( - "SwerveDive/Telemetry/desiredChassisSpeeds", SwerveDriveTelemetry.desiredChassisSpeeds); - - /** Log Swerve Drive States to AdvantageKit */ - getModuleStates(); - getDesiredStates(); - Logger.recordOutput( - "SwerveDive/States/RobotRotation", - SwerveDriveTelemetry.rotationUnit == "degrees" - ? Rotation2d.fromDegrees(SwerveDriveTelemetry.robotRotation) - : Rotation2d.fromRadians(SwerveDriveTelemetry.robotRotation)); - } - - /** Simulation periodic function */ - @Override - public void simulationPeriodic() {} - - /** - * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation - * rate, and calculates and commands module states accordingly. Can use either open-loop or - * closed-loop velocity control for the wheel velocities. Also has field- and robot-relative - * modes, which affect how the translation vector is used. - * - * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in - * meters per second. In robot-relative mode, positive x is torwards the bow (front) and - * positive y is torwards port (left). In field-relative mode, positive x is away from the - * alliance wall (field North) and positive y is torwards the left wall when looking through - * the driver station glass (field West). - * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by - * field/robot relativity. - * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. - */ - public void drive(Translation2d translation, double rotation, boolean fieldRelative) { - - // Open loop is disabled since it shouldn't be used most of the time. - swerveDrive.drive(translation, rotation, fieldRelative, false); - } - - /** - * Drive the robot given a chassis field oriented velocity. - * - * @param velocity Velocity according to the field. - */ - public void driveFieldOriented(ChassisSpeeds velocity) { - swerveDrive.driveFieldOriented(velocity); - } - - /** - * Drive according to the chassis robot oriented velocity. - * - * @param velocity Robot oriented {@link ChassisSpeeds} - */ - public void drive(ChassisSpeeds velocity) { - swerveDrive.drive(velocity); - } - - /************************************************************************* */ - /* COMMAND SECTION -- Swerve-only Commands */ - - /** - * Command to drive the robot using translative values and heading as a setpoint. - * - * @param translationX Translation in the X direction. Cubed for smoother controls. - * @param translationY Translation in the Y direction. Cubed for smoother controls. - * @param headingX Heading X to calculate angle of the joystick. - * @param headingY Heading Y to calculate angle of the joystick. - * @return Drive command. - */ - public Command driveCommand( - DoubleSupplier translationX, - DoubleSupplier translationY, - DoubleSupplier headingX, - DoubleSupplier headingY) { - // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for - // this kind of control. - return run( - () -> { - Translation2d scaledInputs = - SwerveMath.scaleTranslation( - new Translation2d(translationX.getAsDouble(), translationY.getAsDouble()), 0.8); - - // Make the robot move - driveFieldOriented( - swerveDrive.swerveController.getTargetSpeeds( - scaledInputs.getX(), - scaledInputs.getY(), - headingX.getAsDouble(), - headingY.getAsDouble(), - swerveDrive.getOdometryHeading().getRadians(), - swerveDrive.getMaximumVelocity())); - }); - } - - /** - * Command to drive the robot using translative values and heading as angular velocity. NOTE: - * Alternate drive command constructor - * - * @param translationX Translation in the X direction. Cubed for smoother controls. - * @param translationY Translation in the Y direction. Cubed for smoother controls. - * @param angularRotationX Angular velocity of the robot to set. Cubed for smoother controls. - * @return Drive command. - */ - public Command driveCommand( - DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) { - return run( - () -> { - // Make the robot move - swerveDrive.drive( - SwerveMath.scaleTranslation( - new Translation2d( - translationX.getAsDouble() * swerveDrive.getMaximumVelocity(), - translationY.getAsDouble() * swerveDrive.getMaximumVelocity()), - 0.8), - Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumAngularVelocity(), - true, - false); - }); - } - - /** - * Command to drive the robot using translative values and heading as a setpoint. - * - * @param translationX Translation in the X direction. - * @param translationY Translation in the Y direction. - * @param rotation Rotation as a value between [-1, 1] converted to radians. - * @return Drive command. - */ - public Command simDriveCommand( - DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier rotation) { - // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for - // this kind of control. - return run( - () -> { - // Make the robot move - driveFieldOriented( - swerveDrive.swerveController.getTargetSpeeds( - translationX.getAsDouble(), - translationY.getAsDouble(), - rotation.getAsDouble() * Math.PI, - swerveDrive.getOdometryHeading().getRadians(), - swerveDrive.getMaximumVelocity())); - }); - } - - /** - * Get the path follower with events. - * - * @param pathName PathPlanner path name. - * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. - */ - public Command getAutonomousCommand(String pathName) { - // Create a path following command using AutoBuilder. This will also trigger event markers. - return new PathPlannerAuto(pathName); - } - - /** - * Use PathPlanner Path finding to go to a point on the field. - * - * @param pose Target {@link Pose2d} to go to. - * @return PathFinding command - */ - public Command driveToPose(Pose2d pose) { - // Create the constraints to use while pathfinding - PathConstraints constraints = - new PathConstraints(kMaxLinearSpeed, kMaxLinearAccel, kMaxAngularSpeed, kMaxAngularAccel); - - // Since AutoBuilder is configured, we can use it to build pathfinding commands - return AutoBuilder.pathfindToPose( - pose, - constraints, - 0.0, // Goal end velocity in meters/sec - 0.0 // Rotation delay distance in meters. This is how far the robot should travel before - // attempting to rotate. - ); - } - - /** - * Command to characterize the robot drive motors using SysId - * - * @return SysId Drive Command - */ - public Command sysIdDriveMotorCommand() { - return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setDriveSysIdRoutine(new Config(), this, swerveDrive, kMaxV), - kDelay, - kQuasiTimeout, - kDynamicTimeout); - } - - /** - * Command to characterize the robot angle motors using SysId - * - * @return SysId Angle Command - */ - public Command sysIdAngleMotorCommand() { - return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setAngleSysIdRoutine(new Config(), this, swerveDrive), - kDelay, - kQuasiTimeout, - kDynamicTimeout); - } - - /** - * Returns a Command that centers the modules of the SwerveDrive subsystem. - * - * @return a Command that centers the modules of the SwerveDrive subsystem - */ - public Command centerModulesCommand() { - return run(() -> Arrays.asList(swerveDrive.getModules()).forEach(it -> it.setAngle(0.0))); - } - - /** - * Returns a Command that drives the swerve drive to a specific distance at a given speed. - * - * @param distanceInMeters the distance to drive in meters - * @param speedInMetersPerSecond the speed at which to drive in meters per second - * @return a Command that drives the swerve drive to a specific distance at a given speed - */ - public Command driveToDistanceCommand(double distanceInMeters, double speedInMetersPerSecond) { - return Commands.deferredProxy( - () -> - Commands.run(() -> drive(new ChassisSpeeds(speedInMetersPerSecond, 0, 0)), this) - .until( - () -> - swerveDrive.getPose().getTranslation().getDistance(new Translation2d(0, 0)) - > distanceInMeters)); - } - - /************************************************************************* */ - /* UTILITY SECTION -- Utility methods */ - - /** - * Get the swerve drive kinematics object. - * - * @return {@link SwerveDriveKinematics} of the swerve drive. - */ - @AutoLogOutput(key = "Odometry/Kinematics") - public SwerveDriveKinematics getKinematics() { - return swerveDrive.kinematics; - } - - /** - * Gets the current pose (position and rotation) of the robot, as reported by odometry. - * - * @return The robot's pose - */ - @AutoLogOutput(key = "Odometry/RobotPose") - public Pose2d getPose() { - return swerveDrive.getPose(); - } - - /** - * Gets the current yaw angle of the robot, as reported by the swerve pose estimator in the - * underlying drivebase. Note, this is not the raw gyro reading, this may be corrected from calls - * to resetOdometry(). - * - * @return The yaw angle - */ - @AutoLogOutput(key = "Odometry/Heading") - public Rotation2d getHeading() { - return getPose().getRotation(); - } - - /** - * Gets the current field-relative velocity (x, y and omega) of the robot - * - * @return A ChassisSpeeds object of the current field-relative velocity - */ - @AutoLogOutput(key = "Odometry/FieldVelocity") - public ChassisSpeeds getFieldVelocity() { - return swerveDrive.getFieldVelocity(); - } - - /** - * Gets the current velocity (x, y and omega) of the robot - * - * @return A {@link ChassisSpeeds} object of the current velocity - */ - @AutoLogOutput(key = "Odometry/RobotVelocity") - public ChassisSpeeds getRobotVelocity() { - return swerveDrive.getRobotVelocity(); - } - - /** - * Gets the current pitch angle of the robot, as reported by the imu. - * - * @return The heading as a {@link Rotation2d} angle - */ - @AutoLogOutput(key = "Odometry/Pitch") - public Rotation2d getPitch() { - return swerveDrive.getPitch(); - } - - /** - * Set chassis speeds with closed-loop velocity control. - * - * @param chassisSpeeds Chassis Speeds to set. - */ - public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { - swerveDrive.setChassisSpeeds(chassisSpeeds); - } - - /** - * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when - * calling this method. However, if either gyro angle or module position is reset, this must be - * called in order for odometry to keep working. - * - * @param initialHolonomicPose The pose to set the odometry to - */ - public void resetOdometry(Pose2d initialHolonomicPose) { - swerveDrive.resetOdometry(initialHolonomicPose); - } - - /** - * Post the trajectory to the field. - * - * @param trajectory The trajectory to post. - */ - public void postTrajectory(Trajectory trajectory) { - swerveDrive.postTrajectory(trajectory); - } - - /** - * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. - */ - public void zeroGyro() { - swerveDrive.zeroGyro(); - } - - /** - * Checks if the alliance is red, defaults to false if alliance isn't available. - * - * @return true if the red alliance, false if blue. Defaults to false if none is available. - */ - private boolean isRedAlliance() { - var alliance = DriverStation.getAlliance(); - return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; - } - - /** - * This will zero (calibrate) the robot to assume the current position is facing forward - * - *

If red alliance rotate the robot 180 after the drviebase zero command - */ - public void zeroGyroWithAlliance() { - if (isRedAlliance()) { - zeroGyro(); - // Set the pose 180 degrees - resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180))); - } else { - zeroGyro(); - } - } - - /** - * Sets the drive motors to brake/coast mode. - * - * @param brake True to set motors to brake mode, false for coast. - */ - public void setMotorBrake(boolean brake) { - swerveDrive.setMotorIdleMode(brake); - } - - /** - * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which - * direction. The other for the angle of the robot. - * - * @param xInput X joystick input for the robot to move in the X direction. - * @param yInput Y joystick input for the robot to move in the Y direction. - * @param headingX X joystick which controls the angle of the robot. - * @param headingY Y joystick which controls the angle of the robot. - * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. - */ - public ChassisSpeeds getTargetSpeeds( - double xInput, double yInput, double headingX, double headingY) { - Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); - return swerveDrive.swerveController.getTargetSpeeds( - scaledInputs.getX(), - scaledInputs.getY(), - headingX, - headingY, - getHeading().getRadians(), - swerveDrive.getMaximumVelocity()); - } - - /** - * Get the chassis speeds based on controller input of 1 joystick and one angle. Control the robot - * at an offset of 90deg. - * - * @param xInput X joystick input for the robot to move in the X direction. - * @param yInput Y joystick input for the robot to move in the Y direction. - * @param angle The angle in as a {@link Rotation2d}. - * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. - */ - public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) { - Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); - - return swerveDrive.swerveController.getTargetSpeeds( - scaledInputs.getX(), - scaledInputs.getY(), - angle.getRadians(), - getHeading().getRadians(), - swerveDrive.getMaximumVelocity()); - } - - /** - * Get the {@link SwerveController} in the swerve drive. - * - * @return {@link SwerveController} from the {@link SwerveDrive}. - */ - public SwerveController getSwerveController() { - return swerveDrive.swerveController; - } - - /** - * Get the {@link SwerveDriveConfiguration} object. - * - * @return The {@link SwerveDriveConfiguration} for the current drive. - */ - @AutoLogOutput(key = "SwerveDrive/Config") - public SwerveDriveConfiguration getSwerveDriveConfiguration() { - return swerveDrive.swerveDriveConfiguration; - } - - /** Lock the swerve drive to prevent it from moving. */ - public void lock() { - swerveDrive.lockPose(); - } - - /** Add a fake vision reading for testing purposes. */ - public void addFakeVisionReading() { - swerveDrive.addVisionMeasurement( - new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp()); - } - - /** - * Sets the maximum speed of the swerve drive. - * - * @param maximumSpeedInMetersPerSecond the maximum speed to set for the swerve drive in meters - * per second - */ - public void setMaximumSpeed(double maximumSpeedInMetersPerSecond) { - swerveDrive.setMaximumSpeed( - maximumSpeedInMetersPerSecond, - false, - swerveDrive.swerveDriveConfiguration.physicalCharacteristics.optimalVoltage); - } - - /** - * Replaces the swerve module feedforward with a new SimpleMotorFeedforward object. - * - * @param kS the static gain of the feedforward - * @param kV the velocity gain of the feedforward - * @param kA the acceleration gain of the feedforward - */ - public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) { - swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA)); - } - - /** Returns the module states (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "SwerveDive/States/Measured") - private SwerveModuleState[] getModuleStates() { - SwerveModuleState[] states = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - states[i] = - new SwerveModuleState( - SwerveDriveTelemetry.measuredStates[(i * 2) + 1], - Rotation2d.fromDegrees(SwerveDriveTelemetry.measuredStates[i * 2])); - } - return states; - } - - /** Returns the desired states (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "SwerveDive/States/Desired") - private SwerveModuleState[] getDesiredStates() { - SwerveModuleState[] states = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - states[i] = - new SwerveModuleState( - SwerveDriveTelemetry.desiredStates[(i * 2) + 1], - Rotation2d.fromDegrees(SwerveDriveTelemetry.desiredStates[i * 2])); - } - return states; - } - - /************************************************************************* */ - /** 2024 SEASON-SPECIFIC FUNCTIONS, INCLUDED AS EXAMPLES */ - - /** - * Aim the robot at the speaker. - * - * @param tolerance Tolerance in degrees. - * @return Command to turn the robot to the speaker. - */ - public Command aimAtSpeaker(double tolerance) { - SwerveController controller = swerveDrive.getSwerveController(); - return run(() -> { - drive( - ChassisSpeeds.fromFieldRelativeSpeeds( - 0, - 0, - controller.headingCalculate( - getHeading().getRadians(), - vision.getSpeakerYaw(swerveDrive.getOdometryHeading()).getRadians()), - getHeading())); - }) - .until( - () -> - Math.abs( - vision - .getSpeakerYaw(swerveDrive.getOdometryHeading()) - .minus(getHeading()) - .getDegrees()) - < tolerance); - } - - /** - * Aim the robot at the target returned by PhotonVision. - * - * @param camera {@link PhotonCamera} to communicate with. - * @return A {@link Command} which will run the alignment. - */ - public Command aimAtTarget(PhotonCamera camera) { - - return run( - () -> { - PhotonPipelineResult result = camera.getLatestResult(); - if (result.hasTargets()) { - drive( - getTargetSpeeds( - 0, - 0, - Rotation2d.fromDegrees( - result - .getBestTarget() - .getYaw()))); // Not sure if this will work, more math may be required. - } - }); - } -} +// // Copyright (c) 2024 Az-FIRST +// // http://github.com/AZ-First +// // +// // This program is free software; you can redistribute it and/or +// // modify it under the terms of the GNU General Public License +// // version 3 as published by the Free Software Foundation or +// // available in the root directory of this project. +// // +// // 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. +// // +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. +// // +// // NOTE: This module based on the YAGSL Example Project + +// package frc.robot.subsystems.swervedrive.yagsl_old; + +// import static frc.robot.Constants.DrivebaseConstants.*; + +// import com.pathplanner.lib.auto.AutoBuilder; +// import com.pathplanner.lib.commands.PathPlannerAuto; +// import com.pathplanner.lib.path.PathConstraints; +// import com.pathplanner.lib.path.PathPlannerPath; +// import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +// import com.pathplanner.lib.util.ReplanningConfig; +// import edu.wpi.first.math.controller.SimpleMotorFeedforward; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.kinematics.ChassisSpeeds; +// import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +// import edu.wpi.first.math.kinematics.SwerveModuleState; +// import edu.wpi.first.math.trajectory.Trajectory; +// import edu.wpi.first.wpilibj.DriverStation; +// import edu.wpi.first.wpilibj.Timer; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.Commands; +// import edu.wpi.first.wpilibj2.command.SubsystemBase; +// import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +// import frc.robot.Constants.AutonConstants; +// import frc.robot.subsystems.vision.Vision; +// import java.io.File; +// import java.util.Arrays; +// import java.util.function.DoubleSupplier; +// import org.littletonrobotics.junction.AutoLogOutput; +// import org.littletonrobotics.junction.Logger; +// import org.photonvision.PhotonCamera; +// import org.photonvision.targeting.PhotonPipelineResult; +// import swervelib.SwerveController; +// import swervelib.SwerveDrive; +// import swervelib.SwerveDriveTest; +// import swervelib.math.SwerveMath; +// import swervelib.parser.SwerveControllerConfiguration; +// import swervelib.parser.SwerveDriveConfiguration; +// import swervelib.parser.SwerveParser; +// import swervelib.telemetry.SwerveDriveTelemetry; +// import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; + +// /** YAGSLSwerve module */ +// public class YAGSLSwerve extends SubsystemBase { + +// /** Swerve drive object */ +// private final SwerveDrive swerveDrive; + +// /** PhotonVision class to keep an accurate odometry */ +// private Vision vision; + +// /** Enable vision odometry updates while driving */ +// private final boolean visionDriveTest = false; + +// /** +// * Initialize {@link SwerveDrive} with the directory provided. +// * +// * @param directory Directory of swerve drive config files. +// */ +// public YAGSLSwerve(File directory) { + +// // Angle conversion factor is 360 / (GEAR RATIO * ENCODER RESOLUTION) +// double angleConversionFactor = +// SwerveMath.calculateDegreesPerSteeringRotation(kTurnGearRatio); + +// // Motor conversion factor is (PI * WHEEL DIAMETER IN METERS) / (GEAR RATIO * ENCODER +// // RESOLUTION) +// double driveConversionFactor = +// SwerveMath.calculateMetersPerRotation(kWheelRadius * 2.0, kDriveGearRatio); + +// // Output to console and to AdvantageKit +// System.out.println("\"conversionFactors\": {"); +// System.out.println("\t\"angle\": {\"factor\": " + angleConversionFactor + " },"); +// System.out.println("\t\"drive\": {\"factor\": " + driveConversionFactor + " }"); +// System.out.println("}"); +// Logger.recordOutput("SwerveDive/ConversionFactors/Angle", angleConversionFactor); +// Logger.recordOutput("SwerveDive/ConversionFactors/Drive", driveConversionFactor); + +// // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being +// // created. +// SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; +// try { +// swerveDrive = new SwerveParser(directory).createSwerveDrive(kMaxLinearSpeed); +// // Alternative method if you don't want to supply the conversion factor via JSON files. +// // swerveDrive = new SwerveParser_RBSI(directory).createSwerveDrive(maximumSpeed, +// // angleConversionFactor, driveConversionFactor); +// } catch (Exception e) { +// throw new RuntimeException(e); +// } +// // Heading correction should only be used while controlling the robot via angle. +// swerveDrive.setHeadingCorrection(false); +// // Disables cosine compensation for simulations since it causes discrepancies not seen in +// real +// // life. +// if (SwerveDriveTelemetry.isSimulation) { +// swerveDrive.setCosineCompensator(false); +// } else { +// swerveDrive.setCosineCompensator(true); +// } + +// // Correct for skew that gets worse as angular velocity increases. Start with a coefficient +// of +// // 0.1 +// swerveDrive.setAngularVelocityCompensation(true, true, 0.1); +// // Enable if you want to resynchronize your absolute encoders and motor encoders periodically +// // when they are not moving. +// swerveDrive.setModuleEncoderAutoSynchronize(false, 1); +// // Set the absolute encoder to be used over the internal encoder and push the offsets onto +// it. +// // Throws warning if not possible +// swerveDrive.pushOffsetsToEncoders(); + +// if (visionDriveTest) { +// setupPhotonVision(); +// // Stop the odometry thread if we are using vision that way we can synchronize updates +// better. +// swerveDrive.stopOdometryThread(); +// } +// setupPathPlanner(); +// } + +// /** +// * Construct the swerve drive. +// * +// * @param driveCfg SwerveDriveConfiguration for the swerve. +// * @param controllerCfg Swerve Controller. +// */ +// public YAGSLSwerve( +// SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { +// swerveDrive = new SwerveDrive(driveCfg, controllerCfg, kMaxLinearSpeed); +// } + +// /** Setup the photon vision class. */ +// public void setupPhotonVision() { +// vision = new Vision(swerveDrive::getPose, swerveDrive.field); +// } + +// /** Setup AutoBuilder for PathPlanner. */ +// public void setupPathPlanner() { +// AutoBuilder.configureHolonomic( +// this::getPose, // Robot pose supplier +// this::resetOdometry, // Method to reset odometry (called if your auto has a starting +// pose) +// this::getRobotVelocity, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE +// this::setChassisSpeeds, // Driver method given ROBOT RELATIVE ChassisSpeeds +// new HolonomicPathFollowerConfig( +// AutonConstants.kAutoTranslatePID, +// AutonConstants.kAutoAnglePID, +// kMaxLinearSpeed, +// kDriveBaseRadius, +// new ReplanningConfig()), +// () -> { +// // Boolean supplier that controls when the path will be mirrored for the red alliance +// // This will flip the path being followed to the red side of the field. +// // THE ORIGIN WILL REMAIN ON THE BLUE SIDE +// var alliance = DriverStation.getAlliance(); +// return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; +// }, +// this // Reference to this subsystem to set requirements +// ); +// } + +// /** Periodic function -- update odometry and log everything */ +// @Override +// public void periodic() { +// // // When vision is enabled we must manually update odometry in SwerveDrive +// // if (visionDriveTest) { +// // swerveDrive.updateOdometry(); +// // vision.updatePoseEstimation(swerveDrive); +// // } + +// /** Log Telemetry Data to AdvantageKit */ +// Logger.recordOutput("SwerveDive/Telemetry/moduleCount", SwerveDriveTelemetry.moduleCount); +// Logger.recordOutput("SwerveDive/Telemetry/wheelLocations", +// SwerveDriveTelemetry.wheelLocations); +// Logger.recordOutput("SwerveDive/Telemetry/measuredStates", +// SwerveDriveTelemetry.measuredStates); +// Logger.recordOutput("SwerveDive/Telemetry/desiredStates", +// SwerveDriveTelemetry.desiredStates); +// Logger.recordOutput("SwerveDive/Telemetry/robotRotation", +// SwerveDriveTelemetry.robotRotation); +// Logger.recordOutput("SwerveDive/Telemetry/maxSpeed", SwerveDriveTelemetry.maxSpeed); +// Logger.recordOutput("SwerveDive/Telemetry/rotationUnit", SwerveDriveTelemetry.rotationUnit); +// Logger.recordOutput("SwerveDive/Telemetry/sizeLeftRight", +// SwerveDriveTelemetry.sizeLeftRight); +// Logger.recordOutput("SwerveDive/Telemetry/sizeFrontBack", +// SwerveDriveTelemetry.sizeFrontBack); +// Logger.recordOutput( +// "SwerveDive/Telemetry/forwardDirection", SwerveDriveTelemetry.forwardDirection); +// Logger.recordOutput( +// "SwerveDive/Telemetry/maxAngularVelocity", SwerveDriveTelemetry.maxAngularVelocity); +// Logger.recordOutput( +// "SwerveDive/Telemetry/measuredChassisSpeeds", +// SwerveDriveTelemetry.measuredChassisSpeeds); +// Logger.recordOutput( +// "SwerveDive/Telemetry/desiredChassisSpeeds", SwerveDriveTelemetry.desiredChassisSpeeds); + +// /** Log Swerve Drive States to AdvantageKit */ +// getModuleStates(); +// getDesiredStates(); +// Logger.recordOutput( +// "SwerveDive/States/RobotRotation", +// SwerveDriveTelemetry.rotationUnit == "degrees" +// ? Rotation2d.fromDegrees(SwerveDriveTelemetry.robotRotation) +// : Rotation2d.fromRadians(SwerveDriveTelemetry.robotRotation)); +// } + +// /** Simulation periodic function */ +// @Override +// public void simulationPeriodic() {} + +// /** +// * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a +// rotation +// * rate, and calculates and commands module states accordingly. Can use either open-loop or +// * closed-loop velocity control for the wheel velocities. Also has field- and robot-relative +// * modes, which affect how the translation vector is used. +// * +// * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, +// in +// * meters per second. In robot-relative mode, positive x is torwards the bow (front) and +// * positive y is torwards port (left). In field-relative mode, positive x is away from the +// * alliance wall (field North) and positive y is torwards the left wall when looking +// through +// * the driver station glass (field West). +// * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by +// * field/robot relativity. +// * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. +// */ +// public void drive(Translation2d translation, double rotation, boolean fieldRelative) { + +// // Open loop is disabled since it shouldn't be used most of the time. +// swerveDrive.drive(translation, rotation, fieldRelative, false); +// } + +// /** +// * Drive the robot given a chassis field oriented velocity. +// * +// * @param velocity Velocity according to the field. +// */ +// public void driveFieldOriented(ChassisSpeeds velocity) { +// swerveDrive.driveFieldOriented(velocity); +// } + +// /** +// * Drive according to the chassis robot oriented velocity. +// * +// * @param velocity Robot oriented {@link ChassisSpeeds} +// */ +// public void drive(ChassisSpeeds velocity) { +// swerveDrive.drive(velocity); +// } + +// /************************************************************************* */ +// /* COMMAND SECTION -- Swerve-only Commands */ + +// /** +// * Command to drive the robot using translative values and heading as a setpoint. +// * +// * @param translationX Translation in the X direction. Cubed for smoother controls. +// * @param translationY Translation in the Y direction. Cubed for smoother controls. +// * @param headingX Heading X to calculate angle of the joystick. +// * @param headingY Heading Y to calculate angle of the joystick. +// * @return Drive command. +// */ +// public Command driveCommand( +// DoubleSupplier translationX, +// DoubleSupplier translationY, +// DoubleSupplier headingX, +// DoubleSupplier headingY) { +// // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for +// // this kind of control. +// return run( +// () -> { +// Translation2d scaledInputs = +// SwerveMath.scaleTranslation( +// new Translation2d(translationX.getAsDouble(), translationY.getAsDouble()), +// 0.8); + +// // Make the robot move +// driveFieldOriented( +// swerveDrive.swerveController.getTargetSpeeds( +// scaledInputs.getX(), +// scaledInputs.getY(), +// headingX.getAsDouble(), +// headingY.getAsDouble(), +// swerveDrive.getOdometryHeading().getRadians(), +// swerveDrive.getMaximumVelocity())); +// }); +// } + +// /** +// * Command to drive the robot using translative values and heading as angular velocity. NOTE: +// * Alternate drive command constructor +// * +// * @param translationX Translation in the X direction. Cubed for smoother controls. +// * @param translationY Translation in the Y direction. Cubed for smoother controls. +// * @param angularRotationX Angular velocity of the robot to set. Cubed for smoother controls. +// * @return Drive command. +// */ +// public Command driveCommand( +// DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) +// { +// return run( +// () -> { +// // Make the robot move +// swerveDrive.drive( +// SwerveMath.scaleTranslation( +// new Translation2d( +// translationX.getAsDouble() * swerveDrive.getMaximumVelocity(), +// translationY.getAsDouble() * swerveDrive.getMaximumVelocity()), +// 0.8), +// Math.pow(angularRotationX.getAsDouble(), 3) * +// swerveDrive.getMaximumAngularVelocity(), +// true, +// false); +// }); +// } + +// /** +// * Command to drive the robot using translative values and heading as a setpoint. +// * +// * @param translationX Translation in the X direction. +// * @param translationY Translation in the Y direction. +// * @param rotation Rotation as a value between [-1, 1] converted to radians. +// * @return Drive command. +// */ +// public Command simDriveCommand( +// DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier rotation) { +// // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for +// // this kind of control. +// return run( +// () -> { +// // Make the robot move +// driveFieldOriented( +// swerveDrive.swerveController.getTargetSpeeds( +// translationX.getAsDouble(), +// translationY.getAsDouble(), +// rotation.getAsDouble() * Math.PI, +// swerveDrive.getOdometryHeading().getRadians(), +// swerveDrive.getMaximumVelocity())); +// }); +// } + +// /** +// * Get the path follower with events. +// * +// * @param pathName PathPlanner path name. +// * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. +// */ +// public Command getAutonomousCommand(String pathName) { +// // Create a path following command using AutoBuilder. This will also trigger event markers. +// return new PathPlannerAuto(pathName); +// } + +// /** +// * Use PathPlanner Path finding to go to a point on the field. +// * +// * @param pose Target {@link Pose2d} to go to. +// * @return PathFinding command +// */ +// public Command driveToPose(Pose2d pose) { +// // Create the constraints to use while pathfinding +// PathConstraints constraints = +// new PathConstraints(kMaxLinearSpeed, kMaxLinearAccel, kMaxAngularSpeed, +// kMaxAngularAccel); + +// // Since AutoBuilder is configured, we can use it to build pathfinding commands +// return AutoBuilder.pathfindToPose( +// pose, +// constraints, +// 0.0, // Goal end velocity in meters/sec +// 0.0 // Rotation delay distance in meters. This is how far the robot should travel before +// // attempting to rotate. +// ); +// } + +// /** +// * Command to characterize the robot drive motors using SysId +// * +// * @return SysId Drive Command +// */ +// public Command sysIdDriveMotorCommand() { +// return SwerveDriveTest.generateSysIdCommand( +// SwerveDriveTest.setDriveSysIdRoutine(new Config(), this, swerveDrive, kMaxV), +// kDelay, +// kQuasiTimeout, +// kDynamicTimeout); +// } + +// /** +// * Command to characterize the robot angle motors using SysId +// * +// * @return SysId Angle Command +// */ +// public Command sysIdAngleMotorCommand() { +// return SwerveDriveTest.generateSysIdCommand( +// SwerveDriveTest.setAngleSysIdRoutine(new Config(), this, swerveDrive), +// kDelay, +// kQuasiTimeout, +// kDynamicTimeout); +// } + +// /** +// * Returns a Command that centers the modules of the SwerveDrive subsystem. +// * +// * @return a Command that centers the modules of the SwerveDrive subsystem +// */ +// public Command centerModulesCommand() { +// return run(() -> Arrays.asList(swerveDrive.getModules()).forEach(it -> it.setAngle(0.0))); +// } + +// /** +// * Returns a Command that drives the swerve drive to a specific distance at a given speed. +// * +// * @param distanceInMeters the distance to drive in meters +// * @param speedInMetersPerSecond the speed at which to drive in meters per second +// * @return a Command that drives the swerve drive to a specific distance at a given speed +// */ +// public Command driveToDistanceCommand(double distanceInMeters, double speedInMetersPerSecond) { +// return Commands.deferredProxy( +// () -> +// Commands.run(() -> drive(new ChassisSpeeds(speedInMetersPerSecond, 0, 0)), this) +// .until( +// () -> +// swerveDrive.getPose().getTranslation().getDistance(new Translation2d(0, +// 0)) +// > distanceInMeters)); +// } + +// /************************************************************************* */ +// /* UTILITY SECTION -- Utility methods */ + +// /** +// * Get the swerve drive kinematics object. +// * +// * @return {@link SwerveDriveKinematics} of the swerve drive. +// */ +// @AutoLogOutput(key = "Odometry/Kinematics") +// public SwerveDriveKinematics getKinematics() { +// return swerveDrive.kinematics; +// } + +// /** +// * Gets the current pose (position and rotation) of the robot, as reported by odometry. +// * +// * @return The robot's pose +// */ +// @AutoLogOutput(key = "Odometry/RobotPose") +// public Pose2d getPose() { +// return swerveDrive.getPose(); +// } + +// /** +// * Gets the current yaw angle of the robot, as reported by the swerve pose estimator in the +// * underlying drivebase. Note, this is not the raw gyro reading, this may be corrected from +// calls +// * to resetOdometry(). +// * +// * @return The yaw angle +// */ +// @AutoLogOutput(key = "Odometry/Heading") +// public Rotation2d getHeading() { +// return getPose().getRotation(); +// } + +// /** +// * Gets the current field-relative velocity (x, y and omega) of the robot +// * +// * @return A ChassisSpeeds object of the current field-relative velocity +// */ +// @AutoLogOutput(key = "Odometry/FieldVelocity") +// public ChassisSpeeds getFieldVelocity() { +// return swerveDrive.getFieldVelocity(); +// } + +// /** +// * Gets the current velocity (x, y and omega) of the robot +// * +// * @return A {@link ChassisSpeeds} object of the current velocity +// */ +// @AutoLogOutput(key = "Odometry/RobotVelocity") +// public ChassisSpeeds getRobotVelocity() { +// return swerveDrive.getRobotVelocity(); +// } + +// /** +// * Gets the current pitch angle of the robot, as reported by the imu. +// * +// * @return The heading as a {@link Rotation2d} angle +// */ +// @AutoLogOutput(key = "Odometry/Pitch") +// public Rotation2d getPitch() { +// return swerveDrive.getPitch(); +// } + +// /** +// * Set chassis speeds with closed-loop velocity control. +// * +// * @param chassisSpeeds Chassis Speeds to set. +// */ +// public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { +// swerveDrive.setChassisSpeeds(chassisSpeeds); +// } + +// /** +// * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset +// when +// * calling this method. However, if either gyro angle or module position is reset, this must be +// * called in order for odometry to keep working. +// * +// * @param initialHolonomicPose The pose to set the odometry to +// */ +// public void resetOdometry(Pose2d initialHolonomicPose) { +// swerveDrive.resetOdometry(initialHolonomicPose); +// } + +// /** +// * Post the trajectory to the field. +// * +// * @param trajectory The trajectory to post. +// */ +// public void postTrajectory(Trajectory trajectory) { +// swerveDrive.postTrajectory(trajectory); +// } + +// /** +// * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. +// */ +// public void zeroGyro() { +// swerveDrive.zeroGyro(); +// } + +// /** +// * Checks if the alliance is red, defaults to false if alliance isn't available. +// * +// * @return true if the red alliance, false if blue. Defaults to false if none is available. +// */ +// private boolean isRedAlliance() { +// var alliance = DriverStation.getAlliance(); +// return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; +// } + +// /** +// * This will zero (calibrate) the robot to assume the current position is facing forward +// * +// *

If red alliance rotate the robot 180 after the drviebase zero command +// */ +// public void zeroGyroWithAlliance() { +// if (isRedAlliance()) { +// zeroGyro(); +// // Set the pose 180 degrees +// resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180))); +// } else { +// zeroGyro(); +// } +// } + +// /** +// * Sets the drive motors to brake/coast mode. +// * +// * @param brake True to set motors to brake mode, false for coast. +// */ +// public void setMotorBrake(boolean brake) { +// swerveDrive.setMotorIdleMode(brake); +// } + +// /** +// * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which +// * direction. The other for the angle of the robot. +// * +// * @param xInput X joystick input for the robot to move in the X direction. +// * @param yInput Y joystick input for the robot to move in the Y direction. +// * @param headingX X joystick which controls the angle of the robot. +// * @param headingY Y joystick which controls the angle of the robot. +// * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. +// */ +// public ChassisSpeeds getTargetSpeeds( +// double xInput, double yInput, double headingX, double headingY) { +// Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); +// return swerveDrive.swerveController.getTargetSpeeds( +// scaledInputs.getX(), +// scaledInputs.getY(), +// headingX, +// headingY, +// getHeading().getRadians(), +// swerveDrive.getMaximumVelocity()); +// } + +// /** +// * Get the chassis speeds based on controller input of 1 joystick and one angle. Control the +// robot +// * at an offset of 90deg. +// * +// * @param xInput X joystick input for the robot to move in the X direction. +// * @param yInput Y joystick input for the robot to move in the Y direction. +// * @param angle The angle in as a {@link Rotation2d}. +// * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. +// */ +// public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) { +// Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); + +// return swerveDrive.swerveController.getTargetSpeeds( +// scaledInputs.getX(), +// scaledInputs.getY(), +// angle.getRadians(), +// getHeading().getRadians(), +// swerveDrive.getMaximumVelocity()); +// } + +// /** +// * Get the {@link SwerveController} in the swerve drive. +// * +// * @return {@link SwerveController} from the {@link SwerveDrive}. +// */ +// public SwerveController getSwerveController() { +// return swerveDrive.swerveController; +// } + +// /** +// * Get the {@link SwerveDriveConfiguration} object. +// * +// * @return The {@link SwerveDriveConfiguration} for the current drive. +// */ +// @AutoLogOutput(key = "SwerveDrive/Config") +// public SwerveDriveConfiguration getSwerveDriveConfiguration() { +// return swerveDrive.swerveDriveConfiguration; +// } + +// /** Lock the swerve drive to prevent it from moving. */ +// public void lock() { +// swerveDrive.lockPose(); +// } + +// /** Add a fake vision reading for testing purposes. */ +// public void addFakeVisionReading() { +// swerveDrive.addVisionMeasurement( +// new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp()); +// } + +// /** +// * Sets the maximum speed of the swerve drive. +// * +// * @param maximumSpeedInMetersPerSecond the maximum speed to set for the swerve drive in meters +// * per second +// */ +// public void setMaximumSpeed(double maximumSpeedInMetersPerSecond) { +// swerveDrive.setMaximumSpeed( +// maximumSpeedInMetersPerSecond, +// false, +// swerveDrive.swerveDriveConfiguration.physicalCharacteristics.optimalVoltage); +// } + +// /** +// * Replaces the swerve module feedforward with a new SimpleMotorFeedforward object. +// * +// * @param kS the static gain of the feedforward +// * @param kV the velocity gain of the feedforward +// * @param kA the acceleration gain of the feedforward +// */ +// public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) { +// swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA)); +// } + +// /** Returns the module states (turn angles and drive velocities) for all of the modules. */ +// @AutoLogOutput(key = "SwerveDive/States/Measured") +// private SwerveModuleState[] getModuleStates() { +// SwerveModuleState[] states = new SwerveModuleState[4]; +// for (int i = 0; i < 4; i++) { +// states[i] = +// new SwerveModuleState( +// SwerveDriveTelemetry.measuredStates[(i * 2) + 1], +// Rotation2d.fromDegrees(SwerveDriveTelemetry.measuredStates[i * 2])); +// } +// return states; +// } + +// /** Returns the desired states (turn angles and drive velocities) for all of the modules. */ +// @AutoLogOutput(key = "SwerveDive/States/Desired") +// private SwerveModuleState[] getDesiredStates() { +// SwerveModuleState[] states = new SwerveModuleState[4]; +// for (int i = 0; i < 4; i++) { +// states[i] = +// new SwerveModuleState( +// SwerveDriveTelemetry.desiredStates[(i * 2) + 1], +// Rotation2d.fromDegrees(SwerveDriveTelemetry.desiredStates[i * 2])); +// } +// return states; +// } + +// /************************************************************************* */ +// /** 2024 SEASON-SPECIFIC FUNCTIONS, INCLUDED AS EXAMPLES */ + +// /** +// * Aim the robot at the speaker. +// * +// * @param tolerance Tolerance in degrees. +// * @return Command to turn the robot to the speaker. +// */ +// public Command aimAtSpeaker(double tolerance) { +// SwerveController controller = swerveDrive.getSwerveController(); +// return run(() -> { +// drive( +// ChassisSpeeds.fromFieldRelativeSpeeds( +// 0, +// 0, +// controller.headingCalculate( +// getHeading().getRadians(), +// vision.getSpeakerYaw(swerveDrive.getOdometryHeading()).getRadians()), +// getHeading())); +// }) +// .until( +// () -> +// Math.abs( +// vision +// .getSpeakerYaw(swerveDrive.getOdometryHeading()) +// .minus(getHeading()) +// .getDegrees()) +// < tolerance); +// } + +// /** +// * Aim the robot at the target returned by PhotonVision. +// * +// * @param camera {@link PhotonCamera} to communicate with. +// * @return A {@link Command} which will run the alignment. +// */ +// public Command aimAtTarget(PhotonCamera camera) { + +// return run( +// () -> { +// PhotonPipelineResult result = camera.getLatestResult(); +// if (result.hasTargets()) { +// drive( +// getTargetSpeeds( +// 0, +// 0, +// Rotation2d.fromDegrees( +// result +// .getBestTarget() +// .getYaw()))); // Not sure if this will work, more math may be +// required. +// } +// }); +// } +// } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index a94544d9..996600ab 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -1,5 +1,7 @@ // Copyright (c) 2024 Az-FIRST // http://github.com/AZ-First +// Copyright (c) 2024 FRC 2486 +// http://github.com/Coconuts2486-FRC // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License @@ -10,116 +12,62 @@ // 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. -// -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. -// -// NOTE: This module based on the YAGSL Example Project package frc.robot.subsystems.vision; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; +import static frc.robot.Constants.AprilTagConstants.*; +import static frc.robot.Constants.VisionConstants.*; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import frc.robot.Constants.AprilTagConstants; -import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; -import frc.robot.Robot; -import frc.robot.RobotContainer; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.RobotContainer.Cameras; import frc.robot.subsystems.vision.VisionIO.VisionIOInputs; +import frc.robot.util.GeomUtil; +import frc.robot.util.LoggedTunableNumber; import frc.robot.util.VirtualSubsystem; -import java.awt.Desktop; import java.util.ArrayList; import java.util.HashMap; import java.util.List; import java.util.Map; -import java.util.Optional; import java.util.function.Supplier; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.PhotonUtils; -import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; -import org.photonvision.simulation.VisionSystemSim; -import org.photonvision.targeting.PhotonPipelineResult; +import lombok.experimental.ExtensionMethod; +import org.littletonrobotics.junction.Logger; import org.photonvision.targeting.PhotonTrackedTarget; -import swervelib.SwerveDrive; -import swervelib.telemetry.Alert; -import swervelib.telemetry.Alert.AlertType; -import swervelib.telemetry.SwerveDriveTelemetry; -/** - * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from - * https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads - */ +/** Vision subsystem for AprilTag vision (built upon a virtual subsystem) */ +@ExtensionMethod({GeomUtil.class}) public class Vision extends VirtualSubsystem { - /** Photon Vision Simulation */ - public VisionSystemSim visionSim; - - /** Count of times that the odom thinks we're more than 10meters away from the april tag. */ - private double longDistangePoseEstimationCount = 0; - - /** Current pose from the pose estimator using wheel odometry. */ - private Supplier currentPose; - - /** Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}. */ - private final double maximumAmbiguity = 0.25; - - /** Field from {@link swervelib.SwerveDrive#field} */ - private Field2d field2d; + private static final LoggedTunableNumber timestampOffset = + new LoggedTunableNumber("AprilTagVision/TimestampOffset", -(1.0 / 50.0)); + private static final double demoTagPosePersistenceSecs = 0.5; // Tag sources and layout private final Supplier aprilTagTypeSupplier; private final VisionIO[] io; private final VisionIOInputs[] inputs; + // Bookkeeping for last frame exposure and last tag detection private final Map lastFrameTimes = new HashMap<>(); private final Map lastTagDetectionTimes = new HashMap<>(); - /** - * Constructor for the Vision class. - * - * @param currentPose Current pose supplier, should reference {@link SwerveDrive#getPose()} - * @param field Current field, should be {@link SwerveDrive#field} - */ - public Vision(Supplier currentPose, Field2d field) { - this.currentPose = currentPose; - this.field2d = field; - - if (Robot.isSimulation()) { - visionSim = new VisionSystemSim("Vision"); - visionSim.addAprilTags(AprilTagConstants.aprilTagFieldLayout); - - for (Cameras c : Cameras.values()) { - c.addToVisionSim(visionSim); - } - - openSimCameraViews(); - } - this.aprilTagTypeSupplier = RobotContainer::staticGetAprilTagLayoutType; - this.io = null; - inputs = null; - } + // Something to do with demo? + private Pose3d demoTagPose = null; + private double lastDemoTagPoseTimestamp = 0.0; + public static Pose2d robotPose = null; + public static Pose3d speakerPose = null; + private static Translation3d speakerTranslation = null; + private static Rotation3d robotRotation = null; + private static Pose3d thisSpeakerPose = null; + private static Pose3d thisRobotPose = null; // Class method definition, including inputs - // TODO: Constructor from Euclid public Vision(Supplier aprilTagTypeSupplier, VisionIO... io) { this.aprilTagTypeSupplier = aprilTagTypeSupplier; this.io = io; @@ -134,396 +82,91 @@ public Vision(Supplier aprilTagTypeSupplier, VisionIO... io) } } - @Override - public void periodic() {} - - /** - * Calculates a target pose relative to an AprilTag on the field. - * - * @param aprilTag The ID of the AprilTag. - * @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the - * robot to position itself correctly. - * @return The target pose of the AprilTag. - */ - public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) { - Optional aprilTagPose3d = AprilTagConstants.aprilTagFieldLayout.getTagPose(aprilTag); - if (aprilTagPose3d.isPresent()) { - return aprilTagPose3d.get().toPose2d().transformBy(robotOffset); - } else { - throw new RuntimeException( - "Cannot get AprilTag " - + aprilTag - + " from field " - + AprilTagConstants.aprilTagFieldLayout.toString()); - } - } - - /** - * Update the pose estimation inside of {@link SwerveDrive} with all of the given poses. - * - * @param swerveDrive {@link SwerveDrive} instance. - */ - public void updatePoseEstimation(SwerveSubsystem swerveDrive) { - if (SwerveDriveTelemetry.isSimulation) { - visionSim.update(swerveDrive.getPose()); - } - for (Cameras camera : Cameras.values()) { - Optional poseEst = getEstimatedGlobalPose(camera); - if (poseEst.isPresent()) { - var pose = poseEst.get(); - swerveDrive.addVisionMeasurement( - pose.estimatedPose.toPose2d(), pose.timestampSeconds, getEstimationStdDevs(camera)); - } - } - } - - /** - * Generates the estimated robot pose. Returns empty if: - * - *

    - *
  • No Pose Estimates could be generated - *
  • The generated pose estimate was considered not accurate - *
- * - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate - */ - public Optional getEstimatedGlobalPose(Cameras camera) { - Optional poseEst = filterPose(camera.poseEstimator.update()); - // Uncomment to enable outputting of vision targets in sim. - /* - poseEst.ifPresent(estimatedRobotPose -> field2d.getObject(camera + " est pose") - .setPose(estimatedRobotPose.estimatedPose.toPose2d())); - */ - return poseEst; - } - - /** - * The standard deviations of the estimated pose from {@link - * Vision#getEstimatedGlobalPose(Cameras)}, for use with {@link - * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should - * only be used when there are targets visible. - * - * @param camera Desired camera to get the standard deviation of the estimated pose. - */ - public Matrix getEstimationStdDevs(Cameras camera) { - var poseEst = getEstimatedGlobalPose(camera); - var estStdDevs = camera.singleTagStdDevs; - var targets = getLatestResult(camera).getTargets(); - int numTags = 0; - double avgDist = 0; - for (var tgt : targets) { - var tagPose = camera.poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); - if (tagPose.isEmpty()) { + public void periodic() { + for (int i = 0; i < io.length; i++) { + io[i].updateInputs(inputs[i]); + // NOTE: This line causes the RIO to overrun RAM + // Logger.processInputs("AprilTagVision/Inst" + i, inputs[i]); + } + + // Loop over cameras + List allSpeakerPoses = new ArrayList<>(); + List allRobotPoses = new ArrayList<>(); + List allSpeakerPoses3d = new ArrayList<>(); + for (int instanceIndex = 0; instanceIndex < io.length; instanceIndex++) { + var timestamp = inputs[instanceIndex].timestamp; + var latency = inputs[instanceIndex].latency; + var targets = inputs[instanceIndex].targets; + + // Exit if no targets + if (targets == null) { continue; } - numTags++; - if (poseEst.isPresent()) { - avgDist += - PhotonUtils.getDistanceToPose( - poseEst.get().estimatedPose.toPose2d(), tagPose.get().toPose2d()); - } - } - if (numTags == 0) { - return estStdDevs; - } - avgDist /= numTags; - // Decrease std devs if multiple targets are visible - if (numTags > 1) { - estStdDevs = camera.multiTagStdDevs; - } - // Increase std devs based on (average) distance - if (numTags == 1 && avgDist > 4) { - estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - } else { - estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); - } - - return estStdDevs; - } - /** - * Filter pose via the ambiguity and find best estimate between all of the camera's throwing out - * distances more than 10m for a short amount of time. - * - * @param pose Estimated robot pose. - * @return Could be empty if there isn't a good reading. - */ - private Optional filterPose(Optional pose) { - if (pose.isPresent()) { - double bestTargetAmbiguity = 1; // 1 is max ambiguity - for (PhotonTrackedTarget target : pose.get().targetsUsed) { - double ambiguity = target.getPoseAmbiguity(); - if (ambiguity != -1 && ambiguity < bestTargetAmbiguity) { - bestTargetAmbiguity = ambiguity; + // Loop over found targets + for (PhotonTrackedTarget target : targets) { + + // Get the speaker of interest for this alliance + if ((target.getFiducialId() == 4 && DriverStation.getAlliance().get() == Alliance.Red) + || (target.getFiducialId() == 7 + && DriverStation.getAlliance().get() == Alliance.Blue)) { + + // Camera pose is "WHERE IS THE CAMERA AS SEEN FROM TAG"; invert to get tag from camera + Pose3d cameraPose = GeomUtil.toPose3d(target.getBestCameraToTarget().inverse()); + thisRobotPose = + cameraPose.transformBy(Cameras.cameraPoses[instanceIndex].toTransform3d().inverse()); + + // Get the speaker pose from the point of view of the robot + robotRotation = + thisRobotPose.getRotation(); // This is rotation of bot w.r.t. tag coordinate system + speakerTranslation = + thisRobotPose.getTranslation().unaryMinus().rotateBy(robotRotation.unaryMinus()); + thisSpeakerPose = new Pose3d(speakerTranslation, new Rotation3d()); + + allSpeakerPoses3d.add(thisSpeakerPose); + allSpeakerPoses.add(thisSpeakerPose.toPose2d()); + allRobotPoses.add(thisRobotPose.toPose2d()); + + // Log the relevant information to AdvantageKit + Logger.recordOutput("PhotonVision/Speaker_" + inputs[instanceIndex].camname, speakerPose); + Logger.recordOutput( + "PhotonVision/Robot2d_" + inputs[instanceIndex].camname, + robotRotation.toRotation2d()); } } - // ambiguity to high dont use estimate - if (bestTargetAmbiguity > maximumAmbiguity) { - return Optional.empty(); - } - - // est pose is very far from recorded robot pose - if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) - > 1) { - longDistangePoseEstimationCount++; - - // if it calculates that were 10 meter away for more than 10 times in a row its probably - // right - if (longDistangePoseEstimationCount < 10) { - return Optional.empty(); - } - } else { - longDistangePoseEstimationCount = 0; - } - return pose; } - return Optional.empty(); - } - - /** - * Get the latest result from a given Camera. - * - * @param camera Given camera to take the result from. - * @return Photon result from sim or a real camera. - */ - public PhotonPipelineResult getLatestResult(Cameras camera) { - - return Robot.isReal() - ? camera.camera.getLatestResult() - : camera.cameraSim.getCamera().getLatestResult(); - } - - /** - * Get distance of the robot from the AprilTag pose. - * - * @param id AprilTag ID - * @return Distance - */ - public double getDistanceFromAprilTag(int id) { - Optional tag = AprilTagConstants.aprilTagFieldLayout.getTagPose(id); - return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())) - .orElse(-1.0); - } - - /** - * Get tracked target from a camera of AprilTagID - * - * @param id AprilTag ID - * @param camera Camera to check. - * @return Tracked target. - */ - public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) { - PhotonTrackedTarget target = null; - PhotonPipelineResult result = getLatestResult(camera); - if (result.hasTargets()) { - for (PhotonTrackedTarget i : result.getTargets()) { - if (i.getFiducialId() == id) { - target = i; - } - } - } - return target; - } - - /** - * Vision simulation. - * - * @return Vision Simulation - */ - public VisionSystemSim getVisionSim() { - return visionSim; - } - - /** - * Open up the photon vision camera streams on the localhost, assumes running photon vision on - * localhost. - */ - private void openSimCameraViews() { - if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) { - // try - // { - // Desktop.getDesktop().browse(new URI("http://localhost:1182/")); - // Desktop.getDesktop().browse(new URI("http://localhost:1184/")); - // Desktop.getDesktop().browse(new URI("http://localhost:1186/")); - // } catch (IOException | URISyntaxException e) - // { - // e.printStackTrace(); - // } - } - } - - /** Update the {@link Field2d} to include tracked targets/ */ - public void updateVisionField() { - - List targets = new ArrayList(); - for (Cameras c : Cameras.values()) { - if (getLatestResult(c).hasTargets()) { - targets.addAll(getLatestResult(c).targets); - } - } - - List poses = new ArrayList<>(); - for (PhotonTrackedTarget target : targets) { - if (AprilTagConstants.aprilTagFieldLayout.getTagPose(target.getFiducialId()).isPresent()) { - Pose2d targetPose = - AprilTagConstants.aprilTagFieldLayout - .getTagPose(target.getFiducialId()) - .get() - .toPose2d(); - poses.add(targetPose); - } - } - - field2d.getObject("tracked targets").setPoses(poses); - } - - /** Camera Enum to select each camera */ - enum Cameras { - /** Left Camera */ - LEFT_CAM( - "left", - new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)), - new Translation3d( - Units.inchesToMeters(12.056), Units.inchesToMeters(10.981), Units.inchesToMeters(8.44)), - VecBuilder.fill(4, 4, 8), - VecBuilder.fill(0.5, 0.5, 1)), - /** Right Camera */ - RIGHT_CAM( - "right", - new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)), - new Translation3d( - Units.inchesToMeters(12.056), - Units.inchesToMeters(-10.981), - Units.inchesToMeters(8.44)), - VecBuilder.fill(4, 4, 8), - VecBuilder.fill(0.5, 0.5, 1)), - /** Center Camera */ - CENTER_CAM( - "center", - new Rotation3d(0, Units.degreesToRadians(18), 0), - new Translation3d( - Units.inchesToMeters(-4.628), - Units.inchesToMeters(-10.687), - Units.inchesToMeters(16.129)), - VecBuilder.fill(4, 4, 8), - VecBuilder.fill(0.5, 0.5, 1)); - - /** Latency alert to use when high latency is detected. */ - public final Alert latencyAlert; - - /** Camera instance for comms. */ - public final PhotonCamera camera; - - /** Pose estimator for camera. */ - public final PhotonPoseEstimator poseEstimator; - - public final Matrix singleTagStdDevs; - public final Matrix multiTagStdDevs; - - /** Transform of the camera rotation and translation relative to the center of the robot */ - private final Transform3d robotToCamTransform; - - /** Simulated camera instance which only exists during simulations. */ - public PhotonCameraSim cameraSim; - - /** - * Construct a Photon Camera class with help. Standard deviations are fake values, experiment - * and determine estimation noise on an actual robot. - * - * @param name Name of the PhotonVision camera found in the PV UI. - * @param robotToCamRotation {@link Rotation3d} of the camera. - * @param robotToCamTranslation {@link Translation3d} relative to the center of the robot. - * @param singleTagStdDevs Single AprilTag standard deviations of estimated poses from the - * camera. - * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the - * camera. - */ - Cameras( - String name, - Rotation3d robotToCamRotation, - Translation3d robotToCamTranslation, - Matrix singleTagStdDevs, - Matrix multiTagStdDevsMatrix) { - latencyAlert = - new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.WARNING); - - camera = new PhotonCamera(name); - - // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html - robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation); - - poseEstimator = - new PhotonPoseEstimator( - AprilTagConstants.aprilTagFieldLayout, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - camera, - robotToCamTransform); - poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - - this.singleTagStdDevs = singleTagStdDevs; - this.multiTagStdDevs = multiTagStdDevsMatrix; - - if (Robot.isSimulation()) { - SimCameraProperties cameraProp = new SimCameraProperties(); - // A 640 x 480 camera with a 100 degree diagonal FOV. - cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100)); - // Approximate detection noise with average and standard deviation error in pixels. - cameraProp.setCalibError(0.25, 0.08); - // Set the camera image capture framerate (Note: this is limited by robot loop rate). - cameraProp.setFPS(30); - // The average and standard deviation in milliseconds of image data latency. - cameraProp.setAvgLatencyMs(35); - cameraProp.setLatencyStdDevMs(5); - - cameraSim = new PhotonCameraSim(camera, cameraProp); - cameraSim.enableDrawWireframe(true); - } - } - - /** - * Add camera to {@link VisionSystemSim} for simulated photon vision. - * - * @param systemSim {@link VisionSystemSim} to use. - */ - public void addToVisionSim(VisionSystemSim systemSim) { - if (Robot.isSimulation()) { - systemSim.addCamera(cameraSim, robotToCamTransform); - // cameraSim.enableDrawWireframe(true); - } + // Set output value based on number of tags seen + switch ((int) allSpeakerPoses3d.size()) { + case 0: + // If no speaker tags, return a null Pose3d + speakerPose = null; + robotPose = null; + break; + case 1: + // One tag seen, return it + speakerPose = allSpeakerPoses3d.get(0); + robotPose = allRobotPoses.get(0); + break; + default: + // Otherwise, compute the average speakerPose + Translation3d strans0 = allSpeakerPoses3d.get(0).getTranslation(); + Translation3d strans1 = allSpeakerPoses3d.get(1).getTranslation(); + speakerPose = + new Pose3d( + (strans0.getX() + strans1.getX()) / 2., + (strans0.getY() + strans1.getY()) / 2., + (strans0.getZ() + strans1.getZ()) / 2., + new Rotation3d()); + // And then compute the average robotPose + Translation2d rtrans0 = allRobotPoses.get(0).getTranslation(); + Translation2d rtrans1 = allRobotPoses.get(1).getTranslation(); + Rotation2d rrot0 = allRobotPoses.get(0).getRotation(); + Rotation2d rrot1 = allRobotPoses.get(1).getRotation(); + robotPose = + new Pose2d( + (rtrans0.getX() + rtrans1.getX()) / 2.0, + (rtrans0.getY() + rtrans1.getY()) / 2.0, + rrot0.plus(rrot1).div(2.0)); } } - - /*****************************************************************/ - /** 2024 SEASON-SPECIFIC FUNCTIONS, INCLUDED AS EXAMPLES */ - /** - * Get the distance to the speaker. - * - * @return Distance to speaker in meters. - */ - public double getDistanceToSpeaker() { - int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; - // Taken from PhotonUtils.getDistanceToPose - Pose3d speakerAprilTagPose = - AprilTagConstants.aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); - return currentPose - .get() - .getTranslation() - .getDistance(speakerAprilTagPose.toPose2d().getTranslation()); - } - - /** - * Get the yaw to aim at the speaker. - * - * @return {@link Rotation2d} of which you need to achieve. - */ - public Rotation2d getSpeakerYaw(Rotation2d odometryHeading) { - int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; - // Taken from PhotonUtils.getYawToPose() - Pose3d speakerAprilTagPose = - AprilTagConstants.aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); - Translation2d relativeTrl = - speakerAprilTagPose.toPose2d().relativeTo(currentPose.get()).getTranslation(); - return new Rotation2d(relativeTrl.getX(), relativeTrl.getY()).plus(odometryHeading); - } } diff --git a/src/main/java/frc/robot/util/PowerMonitoring.java b/src/main/java/frc/robot/util/PowerMonitoring.java index e69b32b4..a328384e 100644 --- a/src/main/java/frc/robot/util/PowerMonitoring.java +++ b/src/main/java/frc/robot/util/PowerMonitoring.java @@ -22,12 +22,36 @@ public class PowerMonitoring extends VirtualSubsystem { + /** Define the Power Distribution Hardware */ private PowerDistribution m_powerModule = new PowerDistribution( Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerConstants.kPowerModule); + private int NUM_PDH_CHANNELS = m_powerModule.getNumChannels(); private double[] channelCurrents = new double[NUM_PDH_CHANNELS]; + /** Define which robot mechanisms are monitored */ + // DRIVE motor power ports + private final int[] m_drivePowerPorts = { + Ports.FL_DRIVE.getPowerPort(), + Ports.FR_DRIVE.getPowerPort(), + Ports.BL_DRIVE.getPowerPort(), + Ports.BR_DRIVE.getPowerPort() + }; + + // STEER motor power plugged + private final int[] m_steerPowerPorts = { + Ports.FL_ROTATION.getPowerPort(), + Ports.FR_ROTATION.getPowerPort(), + Ports.BL_ROTATION.getPowerPort(), + Ports.BR_ROTATION.getPowerPort() + }; + // Add additional subsystem port enumerations here for combined monitoring + // Example: + private final int[] m_flywheelPowerPorts = { + Ports.FLYWHEEL_LEADER.getPowerPort(), Ports.FLYWHEEL_FOLLOWER.getPowerPort() + }; + /** Periodic Method */ public void periodic() { @@ -46,16 +70,16 @@ public void periodic() { // Compute DRIVE and STEER summed current double driveCurrent = 0.0; double steerCurrent = 0.0; - for (int port : PowerConstants.kDrivePowerPorts) { + for (int port : m_drivePowerPorts) { driveCurrent += channelCurrents[port]; } - for (int port : PowerConstants.kSteerPowerPorts) { + for (int port : m_steerPowerPorts) { steerCurrent += channelCurrents[port]; } // Add current monitoring by subsystem here // Example: double flywheelCurrent = 0.0; - for (int port : PowerConstants.kFlywheelPowerPorts) { + for (int port : m_flywheelPowerPorts) { flywheelCurrent += channelCurrents[port]; } From 589eda4ec143c370e757aef80c414d756bb8fa98 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 6 Nov 2024 12:07:53 -0700 Subject: [PATCH 42/57] Update README's modified: README.md modified: src/main/java/frc/robot/README modified: src/main/java/frc/robot/Robot.java --- README.md | 8 ++++++++ src/main/java/frc/robot/README | 33 ++++++++++++++++++++++-------- src/main/java/frc/robot/Robot.java | 10 ++++----- 3 files changed, 37 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index a0d734dd..fe31cfbc 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,14 @@ # Az-RBSI Arizona's Reference Build and Software Implementation for FRC Robots (read: "A-Z-ribsy") +## Notes + +Will need to update the drive subsystem once AdvantageKit's 2025-beta is +publicly available, as it nicely wraps CTRE's swerve library in the usual +AK formalism (with all the logging!). + +https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/docs/docs/example-projects/talonfx-swerve-template.md + ## Purpose diff --git a/src/main/java/frc/robot/README b/src/main/java/frc/robot/README index 36ba2f23..434d547b 100644 --- a/src/main/java/frc/robot/README +++ b/src/main/java/frc/robot/README @@ -23,7 +23,7 @@ Main.java file. Robot.java - This file is called by Main.java and directs the operation of the robot + This file is called by ``Main.java`` and directs the operation of the robot according to the currently commanded mode (disabled, autonomous, teleop, test, and simulation). Care must be taken when modifying this module to ensure proper operation of the robot. One section that would be useful to @@ -32,10 +32,10 @@ Robot.java RobotContainer.java A file for holding robot subsystems and commands. This file is called - from Robot.java and is where all subsystems and commands should be gathered - and organized including button bindings for operator controls, CANbus and - DIO port enumeration for robot devices, and camera pose information (with - respect to robot center). + from ``Robot.java`` and is where all subsystems and commands should be + gathered and organized including button bindings for operator controls, + CANbus and DIO port enumeration for robot devices, and camera pose + information (with respect to robot center). -------------------- @@ -43,10 +43,22 @@ Subdirectories: commands/ Commands build in separate modules (rather than in subsystem definitions or - inline commands in RobotContainer.java) live in this directory. + inline commands in ``RobotContainer.java``) live in this directory. Organization of files and subdirectories herein is left to the discrection of teams. +generated/ + This directory holds the ``TunerConstants.java`` file produced by Phoenix + Tuner X's swerve generator (you must generate this file for your specific + drivetrain -- the one included in the Az-RBSI is an example only). It must + be modified as described in the AdvantageKit documentation (removal of + final function and final import; modification of ``kSteerInertia`` and + ``kDriveInertia`` constants to be ``0.004`` and ``0.025``, respectively; + optional "Pro" feature, set ``kSteerClosedLoopOutput`` and/or + ``kDriveClosedLoopOutput`` to ``ClosedLoopOutputType.TorqueCurrentFOC``; + characterize the drive and turn feedforward gains [``kS``,``kV``,``kA``] + using the auto routines included in the Az-RBSI). + subsystems/ The hardware definition of subsystems is done in this directory. Organization of files and subdirectories herein is left to the discrection @@ -56,8 +68,13 @@ util/ Various utility functions used by the Az-RBSI are included in this directory. Most teams will not need to modify these routines, but additional utilities deemed necessary by individual teams may be placed - here for organizational purposes. + here for organizational purposes. The EXCEPTIONS are + ``PowerMonitoring.java`` and ``OverrideSwitches.java``, which may be + modified to meet the specific designs of teams. ``PowerMonitoring.java`` + can be modified to add additional mechanisms to the power monitor logging, + and ``OverrideSwitches.java`` can be modified to match any manual override + switches on an operator console as described in that module. -------------------- -Last Modified: 10 Oct 2024, TPEB +Last Modified: 06 Nov 2024, TPEB diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 593377cd..fdb1de8b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,7 +19,6 @@ import frc.robot.Constants.PowerConstants; import frc.robot.RobotContainer.Ports; import frc.robot.util.VirtualSubsystem; -import java.util.List; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LogTable; import org.littletonrobotics.junction.LoggedRobot; @@ -39,7 +38,6 @@ public class Robot extends LoggedRobot { private Command m_autonomousCommand; private RobotContainer m_robotContainer; private Timer m_disabledTimer; - private List virtualSubsystems; /** * This function is run when the robot is first started up and should be used for any @@ -136,10 +134,10 @@ public void disabledInit() { @Override public void disabledPeriodic() { // After WHEEL_LOCK_TIME has elapsed, release the drive brakes - // if (m_disabledTimer.hasElapsed(Constants.DrivebaseConstants.kWheelLockTime)) { - // m_robotContainer.setMotorBrake(false); - // m_disabledTimer.stop(); - // } + if (m_disabledTimer.hasElapsed(Constants.DrivebaseConstants.kWheelLockTime)) { + m_robotContainer.setMotorBrake(false); + m_disabledTimer.stop(); + } } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ From 8e64838c5b4e7224376a0e795116ce5330ff0469 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 6 Nov 2024 12:59:05 -0700 Subject: [PATCH 43/57] Make power monitoring easier Simply pass in subsystems to be monitored in RobotContainer. modified: src/main/java/frc/robot/README modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java modified: src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java modified: src/main/java/frc/robot/util/PowerMonitoring.java new file: src/main/java/frc/robot/util/RBSISubsystem.java --- src/main/java/frc/robot/README | 11 +++--- src/main/java/frc/robot/RobotContainer.java | 9 ++++- .../subsystems/flywheel_example/Flywheel.java | 9 +++-- .../flywheel_example/FlywheelIO.java | 4 +++ .../flywheel_example/FlywheelIOSparkMax.java | 4 +++ .../flywheel_example/FlywheelIOTalonFX.java | 4 +++ .../java/frc/robot/util/PowerMonitoring.java | 35 ++++++++++--------- .../java/frc/robot/util/RBSISubsystem.java | 29 +++++++++++++++ 8 files changed, 79 insertions(+), 26 deletions(-) create mode 100644 src/main/java/frc/robot/util/RBSISubsystem.java diff --git a/src/main/java/frc/robot/README b/src/main/java/frc/robot/README index 434d547b..700dc4bf 100644 --- a/src/main/java/frc/robot/README +++ b/src/main/java/frc/robot/README @@ -68,12 +68,11 @@ util/ Various utility functions used by the Az-RBSI are included in this directory. Most teams will not need to modify these routines, but additional utilities deemed necessary by individual teams may be placed - here for organizational purposes. The EXCEPTIONS are - ``PowerMonitoring.java`` and ``OverrideSwitches.java``, which may be - modified to meet the specific designs of teams. ``PowerMonitoring.java`` - can be modified to add additional mechanisms to the power monitor logging, - and ``OverrideSwitches.java`` can be modified to match any manual override - switches on an operator console as described in that module. + here for organizational purposes. The EXCEPTION is + ``OverrideSwitches.java``, which may be modified to meet the specific + design requirements of teams. ``OverrideSwitches.java`` can be modified + to match any manual override switches on an operator console as described + in that module. -------------------- diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3269512a..dfba5be7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -41,6 +41,7 @@ import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhoton; import frc.robot.util.OverrideSwitches; +import frc.robot.util.PowerMonitoring; import frc.robot.util.RobotDeviceId; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -55,8 +56,9 @@ public class RobotContainer { // Declare the robot subsystems here private final SwerveSubsystem m_drivebase; private final Flywheel m_flywheel; - private final Vision m_vision; private final Accelerometer m_accel; + private final Vision m_vision; + private final PowerMonitoring m_power; // Dashboard inputs private final LoggedDashboardChooser autoChooser; @@ -107,6 +109,11 @@ public RobotContainer() { break; } + // ``PowerMonitoring`` takes all the subsystems for which you wish to have + // power monitoring; DO NOT include ``m_drivebase``, as that is already + // included. + m_power = new PowerMonitoring(m_flywheel); + // Configure the trigger bindings configureBindings(); // Define Auto commands diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java index db24d82c..88d91026 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java @@ -19,13 +19,13 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; +import frc.robot.util.RBSISubsystem; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class Flywheel extends SubsystemBase { +public class Flywheel extends RBSISubsystem { private final FlywheelIO io; private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); private final SimpleMotorFeedforward ffModel; @@ -108,4 +108,9 @@ public double getVelocityRPM() { public double getCharacterizationVelocity() { return inputs.velocityRadPerSec; } + + @Override + public int[] getPowerPorts() { + return io.powerPorts; + } } diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java index 4830fc30..403c200e 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java @@ -16,6 +16,10 @@ import org.littletonrobotics.junction.AutoLog; public interface FlywheelIO { + + // IMPORTANT: Include here all devices that are part of this mechanism! + public final int[] powerPorts = {}; + @AutoLog public static class FlywheelIOInputs { public double positionRad = 0.0; diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java index cf9b845d..5fde4f02 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java @@ -37,6 +37,10 @@ public class FlywheelIOSparkMax implements FlywheelIO { new CANSparkMax(Ports.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless); private final RelativeEncoder encoder = leader.getEncoder(); private final SparkPIDController pid = leader.getPIDController(); + // IMPORTANT: Include here all devices listed above that are part of this mechanism! + public final int[] powerPorts = { + Ports.FLYWHEEL_LEADER.getPowerPort(), Ports.FLYWHEEL_FOLLOWER.getPowerPort() + }; public FlywheelIOSparkMax() { leader.restoreFactoryDefaults(); diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java index 2e087214..2cb4d84d 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java @@ -34,6 +34,10 @@ public class FlywheelIOTalonFX implements FlywheelIO { new TalonFX(Ports.FLYWHEEL_LEADER.getDeviceNumber(), Ports.FLYWHEEL_LEADER.getBus()); private final TalonFX follower = new TalonFX(Ports.FLYWHEEL_FOLLOWER.getDeviceNumber(), Ports.FLYWHEEL_FOLLOWER.getBus()); + // IMPORTANT: Include here all devices listed above that are part of this mechanism! + public final int[] powerPorts = { + Ports.FLYWHEEL_LEADER.getPowerPort(), Ports.FLYWHEEL_FOLLOWER.getPowerPort() + }; private final StatusSignal leaderPosition = leader.getPosition(); private final StatusSignal leaderVelocity = leader.getVelocity(); diff --git a/src/main/java/frc/robot/util/PowerMonitoring.java b/src/main/java/frc/robot/util/PowerMonitoring.java index a328384e..d9d25c87 100644 --- a/src/main/java/frc/robot/util/PowerMonitoring.java +++ b/src/main/java/frc/robot/util/PowerMonitoring.java @@ -22,6 +22,8 @@ public class PowerMonitoring extends VirtualSubsystem { + private final RBSISubsystem[] subsystems; + /** Define the Power Distribution Hardware */ private PowerDistribution m_powerModule = new PowerDistribution( @@ -39,18 +41,18 @@ public class PowerMonitoring extends VirtualSubsystem { Ports.BR_DRIVE.getPowerPort() }; - // STEER motor power plugged + // STEER motor power ports private final int[] m_steerPowerPorts = { Ports.FL_ROTATION.getPowerPort(), Ports.FR_ROTATION.getPowerPort(), Ports.BL_ROTATION.getPowerPort(), Ports.BR_ROTATION.getPowerPort() }; - // Add additional subsystem port enumerations here for combined monitoring - // Example: - private final int[] m_flywheelPowerPorts = { - Ports.FLYWHEEL_LEADER.getPowerPort(), Ports.FLYWHEEL_FOLLOWER.getPowerPort() - }; + + // Class method definition, including inputs of optional subsystems + public PowerMonitoring(RBSISubsystem... subsystems) { + this.subsystems = subsystems; + } /** Periodic Method */ public void periodic() { @@ -76,13 +78,6 @@ public void periodic() { for (int port : m_steerPowerPorts) { steerCurrent += channelCurrents[port]; } - // Add current monitoring by subsystem here - // Example: - double flywheelCurrent = 0.0; - for (int port : m_flywheelPowerPorts) { - flywheelCurrent += channelCurrents[port]; - } - // Log values to AdvantageKit and to SmartDashboard Logger.recordOutput("PowerMonitor/TotalCurrent", totalCurrent); Logger.recordOutput("PowerMonitor/DriveCurrent", driveCurrent); @@ -90,10 +85,16 @@ public void periodic() { SmartDashboard.putNumber("TotalCurrent", totalCurrent); SmartDashboard.putNumber("DriveCurrent", driveCurrent); SmartDashboard.putNumber("SteerCurrent", steerCurrent); - // Add logging for subsystems here - // Example: - Logger.recordOutput("PowerMonitor/FlywheelCurrent", flywheelCurrent); - SmartDashboard.putNumber("FlywheelCurrent", flywheelCurrent); + + // Compute and log any passed-in subsystems + for (RBSISubsystem subsystem : subsystems) { + double subsystemCurrent = 0.0; + for (int port : subsystem.getPowerPorts()) { + subsystemCurrent += channelCurrents[port]; + } + Logger.recordOutput("PowerMonitor/" + subsystem.getName() + "Current", subsystemCurrent); + SmartDashboard.putNumber(subsystem.getName() + "Current", subsystemCurrent); + } // Do something about setting priorities if drawing too much current diff --git a/src/main/java/frc/robot/util/RBSISubsystem.java b/src/main/java/frc/robot/util/RBSISubsystem.java new file mode 100644 index 00000000..7aa52fae --- /dev/null +++ b/src/main/java/frc/robot/util/RBSISubsystem.java @@ -0,0 +1,29 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.util; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class RBSISubsystem extends SubsystemBase { + + /** + * Gets the power ports associated with this Subsystem. + * + * @return Array of power distribution module ports + */ + public int[] getPowerPorts() { + int[] retval = {}; + return retval; + } +} From 4bde82b4b730041fce318831217a462e18d48d2b Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 20 Nov 2024 09:20:37 -0700 Subject: [PATCH 44/57] Some organization, prep for alpha release new file: .github/workflows/pubish_release.yml modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/util/PowerMonitoring.java modified: src/main/java/frc/robot/util/RBSISubsystem.java modified: src/main/java/frc/robot/util/RobotDeviceId.java --- .github/workflows/pubish_release.yml | 15 +++++++++++ src/main/java/frc/robot/RobotContainer.java | 27 ++++++++++++++++--- .../java/frc/robot/util/PowerMonitoring.java | 13 +++++---- .../java/frc/robot/util/RBSISubsystem.java | 6 +++++ .../java/frc/robot/util/RobotDeviceId.java | 6 ++++- 5 files changed, 58 insertions(+), 9 deletions(-) create mode 100644 .github/workflows/pubish_release.yml diff --git a/.github/workflows/pubish_release.yml b/.github/workflows/pubish_release.yml new file mode 100644 index 00000000..6f9abb92 --- /dev/null +++ b/.github/workflows/pubish_release.yml @@ -0,0 +1,15 @@ +name: Publish + +on: + push: + tags: + - "v*" + +jobs: + build: + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v4 + - name: Release + uses: softprops/action-gh-release@v2 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dfba5be7..cdaae60c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -45,6 +45,7 @@ import frc.robot.util.RobotDeviceId; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +/** This is the location for defining robot hardware, commands, and controller button bindings. */ public class RobotContainer { // Define the Driver and, optionally, the Operator/Co-Driver Controllers @@ -109,9 +110,9 @@ public RobotContainer() { break; } - // ``PowerMonitoring`` takes all the subsystems for which you wish to have - // power monitoring; DO NOT include ``m_drivebase``, as that is already - // included. + // ``PowerMonitoring`` takes all the non-drivebase subsystems for which + // you wish to have power monitoring; DO NOT include ``m_drivebase``, + // as that is automatically monitored. m_power = new PowerMonitoring(m_flywheel); // Configure the trigger bindings @@ -256,6 +257,25 @@ public static class Ports { // public static final int INTAKE_SERVO = 0; } + /** Override and Console Toggle Switches ********************************* */ + public static class Overrides { + + // Assumes this controller: https://www.amazon.com/gp/product/B00UUROWWK + // Example from: + // https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2024-build-thread/442736/72 + public static final int DRIVER_SWITCH_0 = 1; + public static final int DRIVER_SWITCH_1 = 2; + public static final int DRIVER_SWITCH_2 = 3; + + public static final int OPERATOR_SWITCH_0 = 8; + public static final int OPERATOR_SWITCH_1 = 9; + public static final int OPERATOR_SWITCH_2 = 10; + public static final int OPERATOR_SWITCH_3 = 11; + public static final int OPERATOR_SWITCH_4 = 12; + + public static final int[] MULTI_TOGGLE = {4, 5}; + } + /** Vision Camera Posses ************************************************* */ public static class Cameras { @@ -263,6 +283,7 @@ public static class Cameras { switch (Constants.getRobot()) { case COMPBOT -> new Pose3d[] { + // Camera #1 new Pose3d( Units.inchesToMeters(-1.0), Units.inchesToMeters(0), diff --git a/src/main/java/frc/robot/util/PowerMonitoring.java b/src/main/java/frc/robot/util/PowerMonitoring.java index d9d25c87..735054e2 100644 --- a/src/main/java/frc/robot/util/PowerMonitoring.java +++ b/src/main/java/frc/robot/util/PowerMonitoring.java @@ -20,6 +20,12 @@ import frc.robot.util.Alert.AlertType; import org.littletonrobotics.junction.Logger; +/** + * Power monitoring virtual subsystem that periodically polls the Power Distribution Module. Each + * port and the sum total currents are compared with limits defined in the ``Constants.java`` file, + * and subsystem total currents are also computed based on the power ports listed in + * ``RobotContainer.java``. + */ public class PowerMonitoring extends VirtualSubsystem { private final RBSISubsystem[] subsystems; @@ -32,16 +38,13 @@ public class PowerMonitoring extends VirtualSubsystem { private int NUM_PDH_CHANNELS = m_powerModule.getNumChannels(); private double[] channelCurrents = new double[NUM_PDH_CHANNELS]; - /** Define which robot mechanisms are monitored */ - // DRIVE motor power ports + // DRIVE and STEER motor power ports private final int[] m_drivePowerPorts = { Ports.FL_DRIVE.getPowerPort(), Ports.FR_DRIVE.getPowerPort(), Ports.BL_DRIVE.getPowerPort(), Ports.BR_DRIVE.getPowerPort() }; - - // STEER motor power ports private final int[] m_steerPowerPorts = { Ports.FL_ROTATION.getPowerPort(), Ports.FR_ROTATION.getPowerPort(), @@ -69,7 +72,7 @@ public void periodic() { } } - // Compute DRIVE and STEER summed current + // Compute DRIVE and STEER summed currents double driveCurrent = 0.0; double steerCurrent = 0.0; for (int port : m_drivePowerPorts) { diff --git a/src/main/java/frc/robot/util/RBSISubsystem.java b/src/main/java/frc/robot/util/RBSISubsystem.java index 7aa52fae..7a5c8e46 100644 --- a/src/main/java/frc/robot/util/RBSISubsystem.java +++ b/src/main/java/frc/robot/util/RBSISubsystem.java @@ -15,6 +15,12 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; +/** + * This class is designed to include Az-RBSI specific methods on top of the standard WPILib + * command-based subsystem classes. All non-drivebase subsystems (e.g., flywheels, arms, elevators, + * etc.) should subclass ``RBSISubsystem`` rather than ``SubsystemBase`` in order to gain access to + * added functionality. + */ public class RBSISubsystem extends SubsystemBase { /** diff --git a/src/main/java/frc/robot/util/RobotDeviceId.java b/src/main/java/frc/robot/util/RobotDeviceId.java index f5b77f81..055491bc 100644 --- a/src/main/java/frc/robot/util/RobotDeviceId.java +++ b/src/main/java/frc/robot/util/RobotDeviceId.java @@ -15,7 +15,11 @@ package frc.robot.util; -/** Class for wrapping Robot / CAN devices with a name and functionality */ +/** + * Class for wrapping Robot / CAN devices with a name and functionality. Included here are both the + * CAN ID for devices and the port on the Power Distribution Module for power monitoring and + * management. + */ public class RobotDeviceId { private final int m_CANDeviceNumber; private final String m_CANBus; From 5330ae4aecd9b3227d7df8e3e00a398e2c85c14f Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 20 Nov 2024 09:38:16 -0700 Subject: [PATCH 45/57] Revert swerve to AK 2024 Swerve Example modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/generated/TunerConstants.java new file: src/main/java/frc/robot/subsystems/drive/Drive.java new file: src/main/java/frc/robot/subsystems/drive/GyroIO.java new file: src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java new file: src/main/java/frc/robot/subsystems/drive/Module.java new file: src/main/java/frc/robot/subsystems/drive/ModuleIO.java new file: src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java new file: src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java new file: src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java new file: src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java renamed: src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java -> src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java renamed: src/main/java/frc/robot/subsystems/swervedrive/SwerveTelemetry.java -> src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java renamed: src/main/java/frc/robot/subsystems/swervedrive/yagsl_old/YAGSLSwerve.java -> src/main/java/frc/robot/subsystems/swervedrive_ignore/yagsl_old/YAGSLSwerve.java --- src/main/java/frc/robot/RobotContainer.java | 4 +- .../frc/robot/generated/TunerConstants.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 282 ++++++++++++++++++ .../frc/robot/subsystems/drive/GyroIO.java | 30 ++ .../robot/subsystems/drive/GyroIOPigeon2.java | 46 +++ .../frc/robot/subsystems/drive/Module.java | 176 +++++++++++ .../frc/robot/subsystems/drive/ModuleIO.java | 50 ++++ .../subsystems/drive/ModuleIOBlended.java | 184 ++++++++++++ .../robot/subsystems/drive/ModuleIOSim.java | 69 +++++ .../subsystems/drive/ModuleIOSparkMax.java | 155 ++++++++++ .../subsystems/drive/ModuleIOTalonFX.java | 195 ++++++++++++ .../SwerveSubsystem.java | 2 +- .../SwerveTelemetry.java | 2 +- .../yagsl_old/YAGSLSwerve.java | 1 + 14 files changed, 1193 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drive/Drive.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIO.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java create mode 100644 src/main/java/frc/robot/subsystems/drive/Module.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIO.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java rename src/main/java/frc/robot/subsystems/{swervedrive => swervedrive_ignore}/SwerveSubsystem.java (99%) rename src/main/java/frc/robot/subsystems/{swervedrive => swervedrive_ignore}/SwerveTelemetry.java (99%) rename src/main/java/frc/robot/subsystems/{swervedrive => swervedrive_ignore}/yagsl_old/YAGSLSwerve.java (99%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cdaae60c..15aa1335 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,8 +35,8 @@ import frc.robot.subsystems.flywheel_example.Flywheel; import frc.robot.subsystems.flywheel_example.FlywheelIO; import frc.robot.subsystems.flywheel_example.FlywheelIOSim; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; -import frc.robot.subsystems.swervedrive.SwerveTelemetry; +import frc.robot.subsystems.swervedrive_ignore.SwerveSubsystem; +import frc.robot.subsystems.swervedrive_ignore.SwerveTelemetry; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhoton; diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index c48599ec..c13cb3ec 100755 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -11,7 +11,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.math.util.Units; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.subsystems.swervedrive_ignore.SwerveSubsystem; // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java new file mode 100644 index 00000000..df6046e1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -0,0 +1,282 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import static edu.wpi.first.units.Units.*; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.pathfinding.Pathfinding; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PathPlannerLogging; +import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.util.LocalADStarAK; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class Drive extends SubsystemBase { + private static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5); + private static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); + private static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); + private static final double DRIVE_BASE_RADIUS = + Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); + private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; + + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final Module[] modules = new Module[4]; // FL, FR, BL, BR + private final SysIdRoutine sysId; + + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); + private Rotation2d rawGyroRotation = new Rotation2d(); + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + private SwerveDrivePoseEstimator poseEstimator = + new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); + + public Drive( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO) { + this.gyroIO = gyroIO; + modules[0] = new Module(flModuleIO, 0); + modules[1] = new Module(frModuleIO, 1); + modules[2] = new Module(blModuleIO, 2); + modules[3] = new Module(brModuleIO, 3); + + // Configure AutoBuilder for PathPlanner + AutoBuilder.configureHolonomic( + this::getPose, + this::setPose, + () -> kinematics.toChassisSpeeds(getModuleStates()), + this::runVelocity, + new HolonomicPathFollowerConfig( + MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), + () -> + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red, + this); + Pathfinding.setPathfinder(new LocalADStarAK()); + PathPlannerLogging.setLogActivePathCallback( + (activePath) -> { + Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + }); + PathPlannerLogging.setLogTargetPoseCallback( + (targetPose) -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); + + // Configure SysId + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> { + for (int i = 0; i < 4; i++) { + modules[i].runCharacterization(voltage.in(Volts)); + } + }, + null, + this)); + } + + public void periodic() { + gyroIO.updateInputs(gyroInputs); + Logger.processInputs("Drive/Gyro", gyroInputs); + for (var module : modules) { + module.periodic(); + } + + // Stop moving when disabled + if (DriverStation.isDisabled()) { + for (var module : modules) { + module.stop(); + } + } + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + } + + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = getModulePositions(); + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + moduleDeltas[moduleIndex] = + new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters + - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + + // Update gyro angle + if (gyroInputs.connected) { + // Use the real gyro angle + rawGyroRotation = gyroInputs.yawPosition; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + } + + // Apply odometry update + poseEstimator.update(rawGyroRotation, modulePositions); + } + + /** + * Runs the drive at the desired velocity. + * + * @param speeds Speeds in meters/sec + */ + public void runVelocity(ChassisSpeeds speeds) { + // Calculate module setpoints + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED); + + // Send setpoints to modules + SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + // The module returns the optimized state, useful for logging + optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); + } + + // Log setpoint states + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); + } + + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + /** + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will + * return to their normal orientations the next time a nonzero velocity is requested. + */ + public void stopWithX() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = getModuleTranslations()[i].getAngle(); + } + kinematics.resetHeadings(headings); + stop(); + } + + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysId.quasistatic(direction); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysId.dynamic(direction); + } + + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getState(); + } + return states; + } + + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getPosition(); + } + return states; + } + + /** Returns the current odometry pose. */ + @AutoLogOutput(key = "Odometry/Robot") + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** Returns the current odometry rotation. */ + public Rotation2d getRotation() { + return getPose().getRotation(); + } + + /** Resets the current odometry pose. */ + public void setPose(Pose2d pose) { + poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + } + + /** + * Adds a vision measurement to the pose estimator. + * + * @param visionPose The pose of the robot as measured by the vision camera. + * @param timestamp The timestamp of the vision measurement in seconds. + */ + public void addVisionMeasurement(Pose2d visionPose, double timestamp) { + poseEstimator.addVisionMeasurement(visionPose, timestamp); + } + + /** Returns the maximum linear speed in meters per sec. */ + public double getMaxLinearSpeedMetersPerSec() { + return MAX_LINEAR_SPEED; + } + + /** Returns the maximum angular speed in radians per sec. */ + public double getMaxAngularSpeedRadPerSec() { + return MAX_ANGULAR_SPEED; + } + + /** Returns an array of module translations. */ + public static Translation2d[] getModuleTranslations() { + return new Translation2d[] { + new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), + new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0), + new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), + new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0) + }; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java new file mode 100644 index 00000000..796d946d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -0,0 +1,30 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface GyroIO { + @AutoLog + public static class GyroIOInputs { + public boolean connected = false; + public Rotation2d yawPosition = new Rotation2d(); + public double yawVelocityRadPerSec = 0.0; + } + + public default void updateInputs(GyroIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java new file mode 100644 index 00000000..bc1ae1f1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -0,0 +1,46 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; + +/** IO implementation for Pigeon2 */ +public class GyroIOPigeon2 implements GyroIO { + private final Pigeon2 pigeon = new Pigeon2(20); + private final StatusSignal yaw = pigeon.getYaw(); + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + + public GyroIOPigeon2() { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(100.0); + yawVelocity.setUpdateFrequency(100.0); + pigeon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java new file mode 100644 index 00000000..ff296dce --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -0,0 +1,176 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; +import org.littletonrobotics.junction.Logger; + +public class Module { + private static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); + + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + private final int index; + + private final SimpleMotorFeedforward driveFeedforward; + private final PIDController driveFeedback; + private final PIDController turnFeedback; + private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop + private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop + private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute + + public Module(ModuleIO io, int index) { + this.io = io; + this.index = index; + + // Switch constants based on mode (the physics simulator is treated as a + // separate robot with different tuning) + switch (Constants.getMode()) { + case REAL: + case REPLAY: + driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); + driveFeedback = new PIDController(0.05, 0.0, 0.0); + turnFeedback = new PIDController(7.0, 0.0, 0.0); + break; + case SIM: + driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); + driveFeedback = new PIDController(0.1, 0.0, 0.0); + turnFeedback = new PIDController(10.0, 0.0, 0.0); + break; + default: + driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); + driveFeedback = new PIDController(0.0, 0.0, 0.0); + turnFeedback = new PIDController(0.0, 0.0, 0.0); + break; + } + + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); + setBrakeMode(true); + } + + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); + + // On first cycle, reset relative turn encoder + // Wait until absolute angle is nonzero in case it wasn't initialized yet + if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { + turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); + } + + // Run closed loop turn control + if (angleSetpoint != null) { + io.setTurnVoltage( + turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); + + // Run closed loop drive control + // Only allowed if closed loop turn control is running + if (speedSetpoint != null) { + // Scale velocity based on turn error + // + // When the error is 90°, the velocity setpoint should be 0. As the wheel turns + // towards the setpoint, its velocity should increase. This is achieved by + // taking the component of the velocity in the direction of the setpoint. + double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); + + // Run drive controller + double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; + io.setDriveVoltage( + driveFeedforward.calculate(velocityRadPerSec) + + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); + } + } + } + + /** Runs the module with the specified setpoint state. Returns the optimized state. */ + public SwerveModuleState runSetpoint(SwerveModuleState state) { + // Optimize state based on current angle + // Controllers run in "periodic" when the setpoint is not null + var optimizedState = SwerveModuleState.optimize(state, getAngle()); + + // Update setpoints, controllers run in "periodic" + angleSetpoint = optimizedState.angle; + speedSetpoint = optimizedState.speedMetersPerSecond; + + return optimizedState; + } + + /** Runs the module with the specified voltage while controlling to zero degrees. */ + public void runCharacterization(double volts) { + // Closed loop turn control + angleSetpoint = new Rotation2d(); + + // Open loop drive control + io.setDriveVoltage(volts); + speedSetpoint = null; + } + + /** Disables all outputs to motors. */ + public void stop() { + io.setTurnVoltage(0.0); + io.setDriveVoltage(0.0); + + // Disable closed loop control for turn and drive + angleSetpoint = null; + speedSetpoint = null; + } + + /** Sets whether brake mode is enabled. */ + public void setBrakeMode(boolean enabled) { + io.setDriveBrakeMode(enabled); + io.setTurnBrakeMode(enabled); + } + + /** Returns the current turn angle of the module. */ + public Rotation2d getAngle() { + if (turnRelativeOffset == null) { + return new Rotation2d(); + } else { + return inputs.turnPosition.plus(turnRelativeOffset); + } + } + + /** Returns the current drive position of the module in meters. */ + public double getPositionMeters() { + return inputs.drivePositionRad * WHEEL_RADIUS; + } + + /** Returns the current drive velocity of the module in meters per second. */ + public double getVelocityMetersPerSec() { + return inputs.driveVelocityRadPerSec * WHEEL_RADIUS; + } + + /** Returns the module position (turn angle and drive position). */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } + + /** Returns the module state (turn angle and drive velocity). */ + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + } + + /** Returns the drive velocity in radians/sec. */ + public double getCharacterizationVelocity() { + return inputs.driveVelocityRadPerSec; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java new file mode 100644 index 00000000..98dde745 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -0,0 +1,50 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface ModuleIO { + @AutoLog + public static class ModuleIOInputs { + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double[] driveCurrentAmps = new double[] {}; + + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double[] turnCurrentAmps = new double[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} + + /** Run the drive motor at the specified voltage. */ + public default void setDriveVoltage(double volts) {} + + /** Run the turn motor at the specified voltage. */ + public default void setTurnVoltage(double volts) {} + + /** Enable or disable brake mode on the drive motor. */ + public default void setDriveBrakeMode(boolean enable) {} + + /** Enable or disable brake mode on the turn motor. */ + public default void setTurnBrakeMode(boolean enable) {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java new file mode 100644 index 00000000..eb888d01 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -0,0 +1,184 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright 2024 FRC 2486 +// https://github.com/Coconuts2486-FRC +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; + +/** + * Module IO implementation for blended TalonFX drive motor controller, SparkMax turn motor + * controller (NEO or NEO 550), and CANcoder. + * + *

To calibrate the absolute encoder offsets, point the modules straight (such that forward + * motion on the drive motor will propel the robot forward) and copy the reported values from the + * absolute encoders using AdvantageScope. These values are logged under + * "/Drive/ModuleX/TurnAbsolutePositionRad" + */ +public class ModuleIONutBlend implements ModuleIO { + // CAN Devices + private final TalonFX driveTalon; + private final CANSparkMax turnSparkMax; + private final CANcoder cancoder; + + // Drive telemetry information + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + // Steer telemetry information + private final StatusSignal turnAbsolutePosition; + private final RelativeEncoder turnRelativeEncoder; + + // Gear ratios for SDS MK4i L2, adjust as necessary + private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + private final double TURN_GEAR_RATIO = 150.0 / 7.0; + + private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; + + /* + * NutBlend Module I/O, using Falcon drive and NEO steer motors + * Based on the ModuleIOTalonFX module, with the SparkMax components + * added in appropriately. + */ + public ModuleIONutBlend(int index) { + switch (index) { + case 0: + driveTalon = new TalonFX(0); + turnSparkMax = new CANSparkMax(1, MotorType.kBrushless); + cancoder = new CANcoder(2); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 1: + driveTalon = new TalonFX(3); + turnSparkMax = new CANSparkMax(4, MotorType.kBrushless); + cancoder = new CANcoder(5); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 2: + driveTalon = new TalonFX(6); + turnSparkMax = new CANSparkMax(7, MotorType.kBrushless); + cancoder = new CANcoder(8); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 3: + driveTalon = new TalonFX(9); + turnSparkMax = new CANSparkMax(10, MotorType.kBrushless); + cancoder = new CANcoder(11); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + default: + throw new RuntimeException("Invalid module index"); + } + + // Drive Configuration + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveTalon.getConfigurator().apply(driveConfig); + setDriveBrakeMode(true); + + // Steer Configuration + turnSparkMax.restoreFactoryDefaults(); + turnSparkMax.setCANTimeout(250); + turnRelativeEncoder = turnSparkMax.getEncoder(); + turnSparkMax.setInverted(isTurnMotorInverted); + turnSparkMax.setSmartCurrentLimit(30); + turnSparkMax.enableVoltageCompensation(12.0); + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod(10); + turnRelativeEncoder.setAverageDepth(2); + turnSparkMax.setCANTimeout(0); + + // CANCoder Configuration + cancoder.getConfigurator().apply(new CANcoderConfiguration()); + + drivePosition = driveTalon.getPosition(); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getSupplyCurrent(); + turnAbsolutePosition = cancoder.getAbsolutePosition(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 100.0, drivePosition); // Required for odometry, use faster rate + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, driveVelocity, driveAppliedVolts, driveCurrent, turnAbsolutePosition); + driveTalon.optimizeBusUtilization(); + turnSparkMax.burnFlash(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll( + drivePosition, driveVelocity, driveAppliedVolts, driveCurrent, turnAbsolutePosition); + + inputs.drivePositionRad = + Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; + inputs.driveVelocityRadPerSec = + Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; + + inputs.turnAbsolutePosition = + Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()) + .minus(absoluteEncoderOffset); + inputs.turnPosition = + Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) + / TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; + } + + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setTurnVoltage(double volts) { + turnSparkMax.setVoltage(volts); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + driveTalon.getConfigurator().apply(config); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java new file mode 100644 index 00000000..39cc551a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -0,0 +1,69 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +/** + * Physics sim implementation of module IO. + * + *

Uses two flywheel sims for the drive and turn motors, with the absolute position initialized + * to a random value. The flywheel sims are not physically accurate, but provide a decent + * approximation for the behavior of the module. + */ +public class ModuleIOSim implements ModuleIO { + private static final double LOOP_PERIOD_SECS = 0.02; + + private DCMotorSim driveSim = new DCMotorSim(DCMotor.getNEO(1), 6.75, 0.025); + private DCMotorSim turnSim = new DCMotorSim(DCMotor.getNEO(1), 150.0 / 7.0, 0.004); + + private final Rotation2d turnAbsoluteInitPosition = new Rotation2d(Math.random() * 2.0 * Math.PI); + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; + + @Override + public void updateInputs(ModuleIOInputs inputs) { + driveSim.update(LOOP_PERIOD_SECS); + turnSim.update(LOOP_PERIOD_SECS); + + inputs.drivePositionRad = driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; + + inputs.turnAbsolutePosition = + new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); + inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; + } + + @Override + public void setDriveVoltage(double volts) { + driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + driveSim.setInputVoltage(driveAppliedVolts); + } + + @Override + public void setTurnVoltage(double volts) { + turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + turnSim.setInputVoltage(turnAppliedVolts); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java new file mode 100644 index 00000000..695755f5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -0,0 +1,155 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.RobotController; + +/** + * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO + * or NEO 550), and analog absolute encoder connected to the RIO + * + *

NOTE: This implementation should be used as a starting point and adapted to different hardware + * configurations (e.g. If using a CANcoder, copy from "ModuleIOTalonFX") + * + *

To calibrate the absolute encoder offsets, point the modules straight (such that forward + * motion on the drive motor will propel the robot forward) and copy the reported values from the + * absolute encoders using AdvantageScope. These values are logged under + * "/Drive/ModuleX/TurnAbsolutePositionRad" + */ +public class ModuleIOSparkMax implements ModuleIO { + // Gear ratios for SDS MK4i L2, adjust as necessary + private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + private static final double TURN_GEAR_RATIO = 150.0 / 7.0; + + private final CANSparkMax driveSparkMax; + private final CANSparkMax turnSparkMax; + + private final RelativeEncoder driveEncoder; + private final RelativeEncoder turnRelativeEncoder; + private final AnalogInput turnAbsoluteEncoder; + + private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; + + public ModuleIOSparkMax(int index) { + switch (index) { + case 0: + driveSparkMax = new CANSparkMax(1, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(2, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(0); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 1: + driveSparkMax = new CANSparkMax(3, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(4, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(1); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 2: + driveSparkMax = new CANSparkMax(5, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(6, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(2); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 3: + driveSparkMax = new CANSparkMax(7, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(8, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(3); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + default: + throw new RuntimeException("Invalid module index"); + } + + driveSparkMax.restoreFactoryDefaults(); + turnSparkMax.restoreFactoryDefaults(); + + driveSparkMax.setCANTimeout(250); + turnSparkMax.setCANTimeout(250); + + driveEncoder = driveSparkMax.getEncoder(); + turnRelativeEncoder = turnSparkMax.getEncoder(); + + turnSparkMax.setInverted(isTurnMotorInverted); + driveSparkMax.setSmartCurrentLimit(40); + turnSparkMax.setSmartCurrentLimit(30); + driveSparkMax.enableVoltageCompensation(12.0); + turnSparkMax.enableVoltageCompensation(12.0); + + driveEncoder.setPosition(0.0); + driveEncoder.setMeasurementPeriod(10); + driveEncoder.setAverageDepth(2); + + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod(10); + turnRelativeEncoder.setAverageDepth(2); + + driveSparkMax.setCANTimeout(0); + turnSparkMax.setCANTimeout(0); + + driveSparkMax.burnFlash(); + turnSparkMax.burnFlash(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + inputs.drivePositionRad = + Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO; + inputs.driveVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO; + inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); + inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()}; + + inputs.turnAbsolutePosition = + new Rotation2d( + turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V() * 2.0 * Math.PI) + .minus(absoluteEncoderOffset); + inputs.turnPosition = + Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) + / TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; + } + + @Override + public void setDriveVoltage(double volts) { + driveSparkMax.setVoltage(volts); + } + + @Override + public void setTurnVoltage(double volts) { + turnSparkMax.setVoltage(volts); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java new file mode 100644 index 00000000..0ada9d66 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -0,0 +1,195 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; + +/** + * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and + * CANcoder + * + *

NOTE: This implementation should be used as a starting point and adapted to different hardware + * configurations (e.g. If using an analog encoder, copy from "ModuleIOSparkMax") + * + *

To calibrate the absolute encoder offsets, point the modules straight (such that forward + * motion on the drive motor will propel the robot forward) and copy the reported values from the + * absolute encoders using AdvantageScope. These values are logged under + * "/Drive/ModuleX/TurnAbsolutePositionRad" + */ +public class ModuleIOTalonFX implements ModuleIO { + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final CANcoder cancoder; + + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Gear ratios for SDS MK4i L2, adjust as necessary + private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + private final double TURN_GEAR_RATIO = 150.0 / 7.0; + + private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; + + public ModuleIOTalonFX(int index) { + switch (index) { + case 0: + driveTalon = new TalonFX(0); + turnTalon = new TalonFX(1); + cancoder = new CANcoder(2); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 1: + driveTalon = new TalonFX(3); + turnTalon = new TalonFX(4); + cancoder = new CANcoder(5); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 2: + driveTalon = new TalonFX(6); + turnTalon = new TalonFX(7); + cancoder = new CANcoder(8); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 3: + driveTalon = new TalonFX(9); + turnTalon = new TalonFX(10); + cancoder = new CANcoder(11); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + default: + throw new RuntimeException("Invalid module index"); + } + + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveTalon.getConfigurator().apply(driveConfig); + setDriveBrakeMode(true); + + var turnConfig = new TalonFXConfiguration(); + turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; + turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + turnTalon.getConfigurator().apply(turnConfig); + setTurnBrakeMode(true); + + cancoder.getConfigurator().apply(new CANcoderConfiguration()); + + drivePosition = driveTalon.getPosition(); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getSupplyCurrent(); + + turnAbsolutePosition = cancoder.getAbsolutePosition(); + turnPosition = turnTalon.getPosition(); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnCurrent = turnTalon.getSupplyCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 100.0, drivePosition, turnPosition); // Required for odometry, use faster rate + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + driveTalon.optimizeBusUtilization(); + turnTalon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + + inputs.drivePositionRad = + Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; + inputs.driveVelocityRadPerSec = + Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; + + inputs.turnAbsolutePosition = + Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()) + .minus(absoluteEncoderOffset); + inputs.turnPosition = + Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO); + inputs.turnVelocityRadPerSec = + Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; + } + + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setTurnVoltage(double volts) { + turnTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + driveTalon.getConfigurator().apply(config); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = + isTurnMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + turnTalon.getConfigurator().apply(config); + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java similarity index 99% rename from src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java rename to src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java index 79decf81..bcb589d9 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java @@ -18,7 +18,7 @@ // NOTE: This module based on the CTRE Phoenix6 examples // https://github.com/CrossTheRoadElec/Phoenix6-Examples -package frc.robot.subsystems.swervedrive; +package frc.robot.subsystems.swervedrive_ignore; import static edu.wpi.first.units.Units.Volts; import static frc.robot.Constants.DrivebaseConstants.*; diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveTelemetry.java b/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java similarity index 99% rename from src/main/java/frc/robot/subsystems/swervedrive/SwerveTelemetry.java rename to src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java index eadff169..817a3f0d 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveTelemetry.java +++ b/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java @@ -18,7 +18,7 @@ // NOTE: This module based on the CTRE Phoenix6 examples // https://github.com/CrossTheRoadElec/Phoenix6-Examples -package frc.robot.subsystems.swervedrive; +package frc.robot.subsystems.swervedrive_ignore; import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; diff --git a/src/main/java/frc/robot/subsystems/swervedrive/yagsl_old/YAGSLSwerve.java b/src/main/java/frc/robot/subsystems/swervedrive_ignore/yagsl_old/YAGSLSwerve.java similarity index 99% rename from src/main/java/frc/robot/subsystems/swervedrive/yagsl_old/YAGSLSwerve.java rename to src/main/java/frc/robot/subsystems/swervedrive_ignore/yagsl_old/YAGSLSwerve.java index 187c601a..b2f316d8 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/yagsl_old/YAGSLSwerve.java +++ b/src/main/java/frc/robot/subsystems/swervedrive_ignore/yagsl_old/YAGSLSwerve.java @@ -1,3 +1,4 @@ +package frc.robot.subsystems.swervedrive_ignore.yagsl_old; // // Copyright (c) 2024 Az-FIRST // // http://github.com/AZ-First // // From 411004a82f5f2bc4c2f2e60c39914060374d8ca1 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 21 Nov 2024 17:53:18 -0700 Subject: [PATCH 46/57] Making progress... DOES NOT BUILD YET Working to reintegrate the AdvantageKit swerve examples in anticipation of the 2025 beta becoming available in the very near future. Accelerometers are all up to snuff now (I think), and the next steps are to integrate the CTRE and YAGSL inputs into the AdvantageKit framework. Also added rough copies of examples from both PathPlanner and Choreo into the codebase, but need to streamline and clean those up. modified: README.md modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java new file: src/main/java/frc/robot/commands/DriveCommands.java modified: src/main/java/frc/robot/generated/TunerConstants.java modified: src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java modified: src/main/java/frc/robot/subsystems/drive/Drive.java modified: src/main/java/frc/robot/subsystems/drive/GyroIO.java modified: src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java modified: src/main/java/frc/robot/subsystems/swervedrive_ignore/yagsl_old/YAGSLSwerve.java --- README.md | 24 ++++ src/main/java/frc/robot/Constants.java | 64 +++++++++- src/main/java/frc/robot/RobotContainer.java | 83 +++++++++++-- .../frc/robot/commands/DriveCommands.java | 79 +++++++++++++ .../frc/robot/generated/TunerConstants.java | 110 +++++++++--------- .../accelerometer/Accelerometer.java | 93 +++++++++++---- .../frc/robot/subsystems/drive/Drive.java | 36 ++++++ .../frc/robot/subsystems/drive/GyroIO.java | 2 + .../robot/subsystems/drive/GyroIOPigeon2.java | 29 ++++- .../subsystems/drive/ModuleIOBlended.java | 4 +- .../yagsl_old/YAGSLSwerve.java | 61 +++++----- 11 files changed, 455 insertions(+), 130 deletions(-) create mode 100644 src/main/java/frc/robot/commands/DriveCommands.java diff --git a/README.md b/README.md index fe31cfbc..461bfa1d 100644 --- a/README.md +++ b/README.md @@ -47,3 +47,27 @@ effective logging for troubleshooting. * [CTRE Phoenix6](https://v6.docs.ctr-electronics.com/en/stable/docs/api-reference/mechanisms/swerve/swerve-overview.html) / [YAGSL](https://yagsl.gitbook.io/yagsl) -- Swerve drive library * [PathPlanner](https://pathplanner.dev/home.html) / [Choreo](https://sleipnirgroup.github.io/Choreo/) -- Autonomous path planning * [PhotonVision](https://docs.photonvision.org/en/latest/) -- Robot vision / tracking + + +## Basic Set Up Information + +The Az-RBSI supports two different drive train options, both based on +AdvantageKit example projects. Each option has a different method of creating +the necessary input files containing details of your setup (motor ID, gear +ratios, etc.): + +* If you are using an entirely CTRE drivebase (*i.e.*, 8x TalonFX-controlled +motors, 4x CANcoder, 1x Pigeon 2.0), you will use the CTRE Tuner X +[Swerve Project Generator](https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html) +to create the necessary ``TunerConstants.java`` file to replace the example +in ``src/main/java/frc/robot/generated``. + +* For all other drivebases (all-REV, blended CTRE/REV, etc.), you will use the +[YAGSL configuration tool](https://broncbotz3481.github.io/YAGSL-Example/) to +create the necessary ``*.json`` files to replace the examples in +``src/main/deploy/swerve``. + +When editing the ``Constants.java`` file, you will need to specify which +drivetrain type is being used via the ``swerveType`` variable near the top of +the file (options are ``SwerveType.PHOENIX6`` or ``SwerveType.YAGSL``) so that +the code reads in the proper configuration file(s). diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5675a311..812c4c6c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,6 +24,7 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; @@ -31,10 +32,12 @@ import frc.robot.generated.TunerConstants; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; +import java.io.File; import java.io.IOException; import java.nio.file.Path; import lombok.Getter; import swervelib.math.Matter; +import swervelib.parser.json.SwerveDriveJson; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -55,6 +58,8 @@ public final class Constants { private static SwerveType swerveType = SwerveType.PHOENIX6; + private static AutoType autoType = AutoType.PATHPLANNER; + public static boolean disableHAL = false; /** Enumerate the robot types (add additional bots here) */ @@ -104,15 +109,44 @@ public static void main(String... args) { /** Enumerate the supported swerve drive types */ public static enum SwerveType { - PHOENIX6, // The all-CTRE Phoenix6 swerve library - YAGSL // The generic YAGSL swerve library (not supported at this time) + PHOENIX6, // The all-CTRE Phoenix6 swerve generator + YAGSL // The generic YAGSL swerve generator } /** Get the current swerve drive type */ - public static SwerveType getSwerve() { + public static SwerveType getSwerveType() { return swerveType; } + /** Location of YAGSL JSON files (if using) */ + public static class YagslConstants { + public static final File yagslDir = new File(Filesystem.getDeployDirectory(), "swerve"); + public static final SwerveDriveJson swerveDriveJson; + + static { + SwerveDriveJson tempJson = null; + try { + tempJson = + new ObjectMapper() + .readValue(new File(yagslDir, "swervedrive.json"), SwerveDriveJson.class); + } catch (Exception e) { + throw new RuntimeException(e); + } + swerveDriveJson = tempJson; + } + } + + /** Enumerate the supported autonomous path planning types */ + public static enum AutoType { + PATHPLANNER, // PathPlanner (https://pathplanner.dev/home.html) + CHOREO // Choreo (https://sleipnirgroup.github.io/Choreo/) + } + + /** Get the current autonomous path planning type */ + public static AutoType getAutoType() { + return autoType; + } + /***************************************************************************/ /* The remainder of this file contains physical and/or software constants for the various subsystems of the robot */ @@ -123,17 +157,18 @@ public static SwerveType getSwerve() { /** Accelerometer Constants ********************************************** */ public static class AccelerometerConstants { - // Insert here the orientation (CCW == +) of the Rio and Pigeon2 from the robot + // Insert here the orientation (CCW == +) of the Rio and IMU from the robot // An angle of "0." means the x-y-z markings on the device match the robot's intrinsic reference // frame. - // NOTE: It is assumed that both the Rio and the Pigeon are mounted such that +Z is UP + // NOTE: It is assumed that both the Rio and the IMU are mounted such that +Z is UP public static final Rotation2d kRioOrientation = switch (getRobot()) { case COMPBOT -> Rotation2d.fromDegrees(0.); case DEVBOT -> Rotation2d.fromDegrees(0.); default -> Rotation2d.fromDegrees(0.); }; - public static final Rotation2d kPigeonOrientation = + // IMU can be one of Pigeon2 or NavX + public static final Rotation2d kIMUOrientation = switch (getRobot()) { case COMPBOT -> Rotation2d.fromDegrees(0.); case DEVBOT -> Rotation2d.fromDegrees(0.); @@ -170,6 +205,23 @@ public static final class AutonConstants { public static final PIDConstants kAutoAnglePID = new PIDConstants(0.4, 0, 0.01); } + /** Choreo Autonomous Action Constants *********************************** */ + public static final class AutoConstants { + public static final double kMaxSpeedMetersPerSecond = 3; + public static final double kMaxAccelerationMetersPerSecondSquared = 3; + public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; + public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; + + public static final double kPXController = 1; + public static final double kPYController = 1; + public static final double kPThetaController = 1; + + // Constraint for the motion profiled robot angle controller + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = + new TrapezoidProfile.Constraints( + kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); + } + /** Drive Base Constants ************************************************* */ public static final class DrivebaseConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 15aa1335..a48e6e53 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,25 +17,30 @@ package frc.robot; +import com.choreo.lib.Choreo; +import com.choreo.lib.ChoreoTrajectory; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.AprilTagConstants; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; +import frc.robot.Constants.AutoConstants; import frc.robot.Constants.DrivebaseConstants; -import frc.robot.generated.TunerConstants; import frc.robot.subsystems.accelerometer.Accelerometer; +import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.flywheel_example.Flywheel; import frc.robot.subsystems.flywheel_example.FlywheelIO; import frc.robot.subsystems.flywheel_example.FlywheelIOSim; -import frc.robot.subsystems.swervedrive_ignore.SwerveSubsystem; import frc.robot.subsystems.swervedrive_ignore.SwerveTelemetry; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; @@ -54,8 +59,11 @@ public class RobotContainer { final CommandXboxController operatorXbox = new CommandXboxController(1); final OverrideSwitches overrides = new OverrideSwitches(2); + Field2d m_field = new Field2d(); + ChoreoTrajectory traj; + // Declare the robot subsystems here - private final SwerveSubsystem m_drivebase; + private final Drive m_drivebase; private final Flywheel m_flywheel; private final Accelerometer m_accel; private final Vision m_vision; @@ -78,35 +86,39 @@ public static AprilTagLayoutType staticGetAprilTagLayoutType() { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + traj = Choreo.getTrajectory("Trajectory"); + + m_field.getObject("traj").setPoses(traj.getInitialPose(), traj.getFinalPose()); + m_field.getObject("trajPoses").setPoses(traj.getPoses()); // Instantiate Robot Subsystems based on RobotType switch (Constants.getMode()) { case REAL: // Real robot, instantiate hardware IO implementations // YAGSL drivebase, get config from deploy directory - m_drivebase = TunerConstants.DriveTrain; + m_drivebase = new Drive(Constants.getSwerveType()); m_flywheel = new Flywheel(new FlywheelIOSim()); // new Flywheel(new FlywheelIOTalonFX()); m_vision = new Vision( this::getAprilTagLayoutType, new VisionIOPhoton(this::getAprilTagLayoutType, "Photon_CAMNAME")); - m_accel = new Accelerometer(m_drivebase.getPigeon2()); + m_accel = new Accelerometer(m_drivebase.getGyro()); break; case SIM: // Sim robot, instantiate physics sim IO implementations - m_drivebase = TunerConstants.DriveTrain; + m_drivebase = new Drive(Constants.getSwerveType()); m_flywheel = new Flywheel(new FlywheelIOSim()); m_vision = new Vision(this::getAprilTagLayoutType); - m_accel = new Accelerometer(m_drivebase.getPigeon2()); + m_accel = new Accelerometer(m_drivebase.getGyro()); break; default: // Replayed robot, disable IO implementations - m_drivebase = TunerConstants.DriveTrain; + m_drivebase = new Drive(Constants.getSwerveType()); m_flywheel = new Flywheel(new FlywheelIO() {}); m_vision = new Vision(this::getAprilTagLayoutType, new VisionIO() {}, new VisionIO() {}); - m_accel = new Accelerometer(m_drivebase.getPigeon2()); + m_accel = new Accelerometer(m_drivebase.getGyro()); break; } @@ -194,9 +206,58 @@ private void configureBindings() { public Command getAutonomousCommand() { // An example command will be run in autonomous - return m_drivebase.getAutoPath("Tests"); + // return m_drivebase.getAutoPath("Tests"); // Use the ``autoChooser`` to define your auto path from the SmartDashboard - // return autoChooser.get(); + return autoChooser.get(); + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommandChoreo() { + var thetaController = new PIDController(AutoConstants.kPThetaController, 0, 0); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + m_drivebase.resetOdometry(traj.getInitialPose()); + + Command swerveCommand = + Choreo.choreoSwerveCommand( + traj, // Choreo trajectory from above + m_drivebase + ::getPose, // A function that returns the current field-relative pose of the robot: + // your + // wheel or vision odometry + new PIDController( + Constants.AutoConstants.kPXController, + 0.0, + 0.0), // PIDController for field-relative X + // translation (input: X error in meters, + // output: m/s). + new PIDController( + Constants.AutoConstants.kPYController, + 0.0, + 0.0), // PIDController for field-relative Y + // translation (input: Y error in meters, + // output: m/s). + thetaController, // PID constants to correct for rotation + // error + (ChassisSpeeds speeds) -> + m_drivebase.drive( // needs to be robot-relative + speeds.vxMetersPerSecond, + speeds.vyMetersPerSecond, + speeds.omegaRadiansPerSecond, + false), + true, // Whether or not to mirror the path based on alliance (this assumes the path is + // created for the blue alliance) + m_drivebase // The subsystem(s) to require, typically your drive subsystem only + ); + + return Commands.sequence( + Commands.runOnce(() -> m_drivebase.resetOdometry(traj.getInitialPose())), + swerveCommand, + m_drivebase.run(() -> m_drivebase.drive(0, 0, 0, false))); } public void setDriveMode() { diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java new file mode 100644 index 00000000..7ed33afa --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -0,0 +1,79 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.drive.Drive; +import java.util.function.DoubleSupplier; + +public class DriveCommands { + private static final double DEADBAND = 0.1; + + private DriveCommands() {} + + /** + * Field relative drive command using two joysticks (controlling linear and angular velocities). + */ + public static Command joystickDrive( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier) { + return Commands.run( + () -> { + // Apply deadband + double linearMagnitude = + MathUtil.applyDeadband( + Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND); + Rotation2d linearDirection = + new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); + + // Square values + linearMagnitude = linearMagnitude * linearMagnitude; + omega = Math.copySign(omega * omega, omega); + + // Calcaulate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + + // Convert to field relative speeds & send command + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega * drive.getMaxAngularSpeedRadPerSec(), + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); + }, + drive); + } +} diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index c13cb3ec..bb70d422 100755 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -20,30 +20,30 @@ public class TunerConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = + public static final Slot0Configs steerGains = new Slot0Configs().withKP(100).withKI(0).withKD(0.2).withKS(0).withKV(1.5).withKA(0); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = + public static final Slot0Configs driveGains = new Slot0Configs().withKP(3).withKI(0).withKD(0).withKS(0).withKV(0).withKA(0); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType steerClosedLoopOutput = + public static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC; // The closed-loop output type to use for the drive motors; // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType driveClosedLoopOutput = + public static final ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC; // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final double kSlipCurrentA = 150.0; + public static final double kSlipCurrentA = 150.0; // Initial configs for the drive and steer motors and the CANcoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = + public static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + public static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() @@ -52,9 +52,9 @@ public class TunerConstants { // stator current limit to help avoid brownouts without impacting performance. .withStatorCurrentLimit(60) .withStatorCurrentLimitEnable(true)); - private static final CANcoderConfiguration cancoderInitialConfigs = new CANcoderConfiguration(); + public static final CANcoderConfiguration cancoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; + public static final Pigeon2Configuration pigeonConfigs = null; // Theoretical free speed (m/s) at 12v applied output; // This needs to be tuned to your individual robot @@ -62,32 +62,32 @@ public class TunerConstants { // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.5714285714285716; + public static final double kCoupleRatio = 3.5714285714285716; - private static final double kDriveGearRatio = 6.122448979591837; - private static final double kSteerGearRatio = 21.428571428571427; - private static final double kWheelRadiusInches = 2; + public static final double kDriveGearRatio = 6.122448979591837; + public static final double kSteerGearRatio = 21.428571428571427; + public static final double kWheelRadiusInches = 2; - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; + public static final boolean kInvertLeftSide = false; + public static final boolean kInvertRightSide = true; - private static final String kCANbusName = "DriveTrain"; - private static final int kPigeonId = 13; + public static final String kCANbusName = "DriveTrain"; + public static final int kPigeonId = 13; // These are only used for simulation - private static final double kSteerInertia = 0.00001; - private static final double kDriveInertia = 0.001; + public static final double kSteerInertia = 0.004; + public static final double kDriveInertia = 0.025; // Simulated voltage necessary to overcome friction - private static final double kSteerFrictionVoltage = 0.25; - private static final double kDriveFrictionVoltage = 0.25; + public static final double kSteerFrictionVoltage = 0.25; + public static final double kDriveFrictionVoltage = 0.25; - private static final SwerveDrivetrainConstants DrivetrainConstants = + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() .withCANbusName(kCANbusName) .withPigeon2Id(kPigeonId) .withPigeon2Configs(pigeonConfigs); - private static final SwerveModuleConstantsFactory ConstantCreator = + public static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() .withDriveMotorGearRatio(kDriveGearRatio) .withSteerMotorGearRatio(kSteerGearRatio) @@ -109,46 +109,46 @@ public class TunerConstants { .withCANcoderInitialConfigs(cancoderInitialConfigs); // Front Left - private static final int kFrontLeftDriveMotorId = 1; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 3; - private static final double kFrontLeftEncoderOffset = -0.303466796875; - private static final boolean kFrontLeftSteerInvert = true; + public static final int kFrontLeftDriveMotorId = 1; + public static final int kFrontLeftSteerMotorId = 2; + public static final int kFrontLeftEncoderId = 3; + public static final double kFrontLeftEncoderOffset = -0.303466796875; + public static final boolean kFrontLeftSteerInvert = true; - private static final double kFrontLeftXPosInches = 10.375; - private static final double kFrontLeftYPosInches = 10.375; + public static final double kFrontLeftXPosInches = 10.375; + public static final double kFrontLeftYPosInches = 10.375; // Front Right - private static final int kFrontRightDriveMotorId = 4; - private static final int kFrontRightSteerMotorId = 5; - private static final int kFrontRightEncoderId = 6; - private static final double kFrontRightEncoderOffset = -0.122802734375; - private static final boolean kFrontRightSteerInvert = true; + public static final int kFrontRightDriveMotorId = 4; + public static final int kFrontRightSteerMotorId = 5; + public static final int kFrontRightEncoderId = 6; + public static final double kFrontRightEncoderOffset = -0.122802734375; + public static final boolean kFrontRightSteerInvert = true; - private static final double kFrontRightXPosInches = 10.375; - private static final double kFrontRightYPosInches = -10.375; + public static final double kFrontRightXPosInches = 10.375; + public static final double kFrontRightYPosInches = -10.375; // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 8; - private static final int kBackLeftEncoderId = 9; - private static final double kBackLeftEncoderOffset = 0.4814453125; - private static final boolean kBackLeftSteerInvert = true; + public static final int kBackLeftDriveMotorId = 7; + public static final int kBackLeftSteerMotorId = 8; + public static final int kBackLeftEncoderId = 9; + public static final double kBackLeftEncoderOffset = 0.4814453125; + public static final boolean kBackLeftSteerInvert = true; - private static final double kBackLeftXPosInches = -10.375; - private static final double kBackLeftYPosInches = 10.375; + public static final double kBackLeftXPosInches = -10.375; + public static final double kBackLeftYPosInches = 10.375; // Back Right - private static final int kBackRightDriveMotorId = 10; - private static final int kBackRightSteerMotorId = 11; - private static final int kBackRightEncoderId = 12; - private static final double kBackRightEncoderOffset = 0.300537109375; - private static final boolean kBackRightSteerInvert = true; + public static final int kBackRightDriveMotorId = 10; + public static final int kBackRightSteerMotorId = 11; + public static final int kBackRightEncoderId = 12; + public static final double kBackRightEncoderOffset = 0.300537109375; + public static final boolean kBackRightSteerInvert = true; - private static final double kBackRightXPosInches = -10.375; - private static final double kBackRightYPosInches = -10.375; + public static final double kBackRightXPosInches = -10.375; + public static final double kBackRightYPosInches = -10.375; - private static final SwerveModuleConstants FrontLeft = + public static final SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, @@ -158,7 +158,7 @@ public class TunerConstants { Units.inchesToMeters(kFrontLeftYPosInches), kInvertLeftSide) .withSteerMotorInverted(kFrontLeftSteerInvert); - private static final SwerveModuleConstants FrontRight = + public static final SwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants( kFrontRightSteerMotorId, kFrontRightDriveMotorId, @@ -168,7 +168,7 @@ public class TunerConstants { Units.inchesToMeters(kFrontRightYPosInches), kInvertRightSide) .withSteerMotorInverted(kFrontRightSteerInvert); - private static final SwerveModuleConstants BackLeft = + public static final SwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants( kBackLeftSteerMotorId, kBackLeftDriveMotorId, @@ -178,7 +178,7 @@ public class TunerConstants { Units.inchesToMeters(kBackLeftYPosInches), kInvertLeftSide) .withSteerMotorInverted(kBackLeftSteerInvert); - private static final SwerveModuleConstants BackRight = + public static final SwerveModuleConstants BackRight = ConstantCreator.createModuleConstants( kBackRightSteerMotorId, kBackRightDriveMotorId, diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index aa281e18..5b611b45 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -16,31 +16,69 @@ import static frc.robot.Constants.AccelerometerConstants.*; import com.ctre.phoenix6.hardware.Pigeon2; +import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.BuiltInAccelerometer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; +import frc.robot.Constants.YagslConstants; import frc.robot.util.VirtualSubsystem; import org.littletonrobotics.junction.Logger; -/** Accelerometer subsystem (built upon a virtual subsystem) */ +/** + * Accelerometer subsystem (built upon a virtual subsystem) + * + *

This virtual subsystem pulls the acceleration values from both the RoboRIO and the swerve's + * IMU (either Pigeon2 or NavX) and logs them to both AdvantageKit and the SmartDashboard. In + * addition to the accelerations, the jerk (a-dot or x-tripple-dot) is computed from the delta + * accelerations. + */ public class Accelerometer extends VirtualSubsystem { private final BuiltInAccelerometer rioAccelerometer = new BuiltInAccelerometer(); private final Pigeon2 pigeonAccelerometer; + private final AHRS navXAccelerometer; // Define the 3D vectors needed to hold values private Translation3d rioAccVector; - private Translation3d pigeonAccVector; + private Translation3d imuAccVector; private Translation3d prevRioAccel = new Translation3d(); - private Translation3d prevPigeonAccel = new Translation3d(); + private Translation3d prevImuAccel = new Translation3d(); private Translation3d rioJerkVector; - private Translation3d pigeonJerkVector; + private Translation3d imuJerkVector; - // Class method definition - public Accelerometer(Pigeon2 pigeonAccelerometer) { - this.pigeonAccelerometer = pigeonAccelerometer; + /** Constructor method, takes the IMU accelerometer instance */ + public Accelerometer(Object accelerometer) { + switch (Constants.getSwerveType()) { + case PHOENIX6: + // CTRE Tuner X drive bases must use a Pigeon2 + pigeonAccelerometer = (Pigeon2) accelerometer; + navXAccelerometer = null; + break; + + case YAGSL: + // Logic checking the type of IMU included in the parsed YAGSL: + if (YagslConstants.swerveDriveJson.imu.type == "pigeon2") { + pigeonAccelerometer = (Pigeon2) accelerometer; + navXAccelerometer = null; + break; + } + + if (YagslConstants.swerveDriveJson.imu.type == "navx" + || YagslConstants.swerveDriveJson.imu.type == "navx_spi") { + navXAccelerometer = (AHRS) accelerometer; + pigeonAccelerometer = null; + break; + } + + default: + // Otherwise kick a message to the console, and set to null + System.out.println("WARNING: Cannot initialize IMU's accelerometer for logging!"); + ; + this.pigeonAccelerometer = null; + this.navXAccelerometer = null; + } } /** Get accelerations, compute jerks, log everything */ @@ -55,33 +93,44 @@ public void periodic() { .rotateBy(new Rotation3d(0., 0., kRioOrientation.getRadians())) .times(9.81); - // Compute the Pigeon's acceleration, rotated as needed - // Pigeon provides accelerations in `g`, from -2 to +2; convert to m/s^2 - pigeonAccVector = - new Translation3d( - pigeonAccelerometer.getAccelerationX().getValueAsDouble(), - pigeonAccelerometer.getAccelerationY().getValueAsDouble(), - pigeonAccelerometer.getAccelerationZ().getValueAsDouble()) - .rotateBy(new Rotation3d(0., 0., kPigeonOrientation.getRadians())) - .times(9.81); + // Compute the IMU's acceleration, rotated as needed + if (pigeonAccelerometer != null) { + // Pigeon provides accelerations in `g`, from -2 to +2; convert to m/s^2 + imuAccVector = + new Translation3d( + pigeonAccelerometer.getAccelerationX().getValueAsDouble(), + pigeonAccelerometer.getAccelerationY().getValueAsDouble(), + pigeonAccelerometer.getAccelerationZ().getValueAsDouble()); + } else if (navXAccelerometer != null) { + // NavX provides accelerations in ... + imuAccVector = + new Translation3d( + navXAccelerometer.getWorldLinearAccelX(), + navXAccelerometer.getWorldLinearAccelY(), + navXAccelerometer.getWorldLinearAccelZ()); + } else { + imuAccVector = new Translation3d(); + } + imuAccVector = + imuAccVector.rotateBy(new Rotation3d(0., 0., kIMUOrientation.getRadians())).times(9.81); // Compute the jerks ((current - prev accel) / loop time) rioJerkVector = rioAccVector.minus(prevRioAccel).div(Constants.loopPeriodSecs); - pigeonJerkVector = pigeonAccVector.minus(prevPigeonAccel).div(Constants.loopPeriodSecs); + imuJerkVector = imuAccVector.minus(prevImuAccel).div(Constants.loopPeriodSecs); // Log everything to both AdvantageKit and SmartDashboard Logger.recordOutput("Acceleration/Rio/Accel_mss", rioAccVector); Logger.recordOutput("Acceleration/Rio/Jerk_msss", rioJerkVector); - Logger.recordOutput("Acceleration/Pigeon/Accel_mss", pigeonAccVector); - Logger.recordOutput("Acceleration/Pigeon/Jerk_msss", pigeonJerkVector); + Logger.recordOutput("Acceleration/IMU/Accel_mss", imuAccVector); + Logger.recordOutput("Acceleration/IMU/Jerk_msss", imuJerkVector); SmartDashboard.putNumber("RioXAccel", rioAccVector.getX()); SmartDashboard.putNumber("RioYAccel", rioAccVector.getY()); - SmartDashboard.putNumber("PigeonXAccel", pigeonAccVector.getX()); - SmartDashboard.putNumber("PigeonYAccel", pigeonAccVector.getY()); + SmartDashboard.putNumber("IMUXAccel", imuAccVector.getX()); + SmartDashboard.putNumber("IMUYAccel", imuAccVector.getY()); // Set the "previous" accelerations to the current for the next loop prevRioAccel = rioAccVector; - prevPigeonAccel = pigeonAccVector; + prevImuAccel = imuAccVector; // Quick logging to see how long this periodic takes long finish = System.nanoTime(); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index df6046e1..dbf437b5 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -37,6 +37,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; +import frc.robot.generated.TunerConstants; import frc.robot.util.LocalADStarAK; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -66,6 +68,23 @@ public class Drive extends SubsystemBase { private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); + // Constructor that takes ENUM type + public Drive(Constants.SwerveType swerveType) { + switch (swerveType) { + case PHOENIX6: + gyroIO = new GyroIOPigeon2(TunerConstants.kPigeonId, TunerConstants.kCANbusName); + modules[0] = new Module(new ModuleIOTalonFX(0), 0); + modules[0] = new Module(new ModuleIOTalonFX(0), 0); + modules[0] = new Module(new ModuleIOTalonFX(0), 0); + modules[0] = new Module(new ModuleIOTalonFX(0), 0); + + case YAGSL: + + default: + } + } + + // Default constructor public Drive( GyroIO gyroIO, ModuleIO flModuleIO, @@ -279,4 +298,21 @@ public static Translation2d[] getModuleTranslations() { new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0) }; } + + public T getGyro() { + return gyroIO.getGyro(); + } + + public double getDriveGearRatio() { + switch (Constants.getSwerveType()) { + case PHOENIX6: + return TunerConstants.kDriveGearRatio; + case YAGSL: + return; + } + } + + public double getTurnGearRatio() { + return 0.0; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 796d946d..10c8c3ec 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -27,4 +27,6 @@ public static class GyroIOInputs { } public default void updateInputs(GyroIOInputs inputs) {} + + public abstract T getGyro(); } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index bc1ae1f1..1e67f958 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -25,18 +25,41 @@ /** IO implementation for Pigeon2 */ public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(20); - private final StatusSignal yaw = pigeon.getYaw(); - private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + private final Pigeon2 pigeon; + private final StatusSignal yaw; + private final StatusSignal yawVelocity; + // Constructor, taking default values public GyroIOPigeon2() { + pigeon = new Pigeon2(20); pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); + yaw = pigeon.getYaw(); + yawVelocity = pigeon.getAngularVelocityZWorld(); yaw.setUpdateFrequency(100.0); yawVelocity.setUpdateFrequency(100.0); pigeon.optimizeBusUtilization(); } + // Constructor, taking deviceID and canBus + public GyroIOPigeon2(int deviceID, String canBus) { + pigeon = new Pigeon2(deviceID, canBus); + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + yaw = pigeon.getYaw(); + yawVelocity = pigeon.getAngularVelocityZWorld(); + yaw.setUpdateFrequency(100.0); + yawVelocity.setUpdateFrequency(100.0); + + pigeon.optimizeBusUtilization(); + } + + // Return the Pigeon2 instance + @SuppressWarnings("unchecked") + public Pigeon2 getGyro() { + return pigeon; + } + @Override public void updateInputs(GyroIOInputs inputs) { inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index eb888d01..447e3909 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -41,7 +41,7 @@ * absolute encoders using AdvantageScope. These values are logged under * "/Drive/ModuleX/TurnAbsolutePositionRad" */ -public class ModuleIONutBlend implements ModuleIO { +public class ModuleIOBlended implements ModuleIO { // CAN Devices private final TalonFX driveTalon; private final CANSparkMax turnSparkMax; @@ -69,7 +69,7 @@ public class ModuleIONutBlend implements ModuleIO { * Based on the ModuleIOTalonFX module, with the SparkMax components * added in appropriately. */ - public ModuleIONutBlend(int index) { + public ModuleIOBlended(int index) { switch (index) { case 0: driveTalon = new TalonFX(0); diff --git a/src/main/java/frc/robot/subsystems/swervedrive_ignore/yagsl_old/YAGSLSwerve.java b/src/main/java/frc/robot/subsystems/swervedrive_ignore/yagsl_old/YAGSLSwerve.java index b2f316d8..e2035742 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive_ignore/yagsl_old/YAGSLSwerve.java +++ b/src/main/java/frc/robot/subsystems/swervedrive_ignore/yagsl_old/YAGSLSwerve.java @@ -1,24 +1,23 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. +// +// NOTE: This module based on the YAGSL Example Project + package frc.robot.subsystems.swervedrive_ignore.yagsl_old; -// // Copyright (c) 2024 Az-FIRST -// // http://github.com/AZ-First -// // -// // This program is free software; you can redistribute it and/or -// // modify it under the terms of the GNU General Public License -// // version 3 as published by the Free Software Foundation or -// // available in the root directory of this project. -// // -// // 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. -// // -// // Copyright (c) FIRST and other WPILib contributors. -// // Open Source Software; you can modify and/or share it under the terms of -// // the WPILib BSD license file in the root directory of this project. -// // -// // NOTE: This module based on the YAGSL Example Project - -// package frc.robot.subsystems.swervedrive.yagsl_old; // import static frc.robot.Constants.DrivebaseConstants.*; @@ -97,17 +96,17 @@ // Logger.recordOutput("SwerveDive/ConversionFactors/Angle", angleConversionFactor); // Logger.recordOutput("SwerveDive/ConversionFactors/Drive", driveConversionFactor); -// // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being -// // created. -// SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; -// try { -// swerveDrive = new SwerveParser(directory).createSwerveDrive(kMaxLinearSpeed); -// // Alternative method if you don't want to supply the conversion factor via JSON files. -// // swerveDrive = new SwerveParser_RBSI(directory).createSwerveDrive(maximumSpeed, -// // angleConversionFactor, driveConversionFactor); -// } catch (Exception e) { -// throw new RuntimeException(e); -// } + // // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being + // // created. + // SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; + // try { + // swerveDrive = new SwerveParser(directory).createSwerveDrive(kMaxLinearSpeed); + // // Alternative method if you don't want to supply the conversion factor via JSON files. + // // swerveDrive = new SwerveParser_RBSI(directory).createSwerveDrive(maximumSpeed, + // // angleConversionFactor, driveConversionFactor); + // } catch (Exception e) { + // throw new RuntimeException(e); + // } // // Heading correction should only be used while controlling the robot via angle. // swerveDrive.setHeadingCorrection(false); // // Disables cosine compensation for simulations since it causes discrepancies not seen in From 0387d4bab64c614c917309c3a5ca7dc57e034f38 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 22 Nov 2024 11:43:49 -0700 Subject: [PATCH 47/57] YAGSL values now available under Tuner names The values from the YAGSL JSON files are now parsed and available under the same variable names as defined in Tuner X's ``TunerConstants.java`` file. When pulling these values from the drivetrain, it just matters which file is pulled from to get the correct constants. new file: src/main/deploy/apriltags/2024-amps.json new file: src/main/deploy/apriltags/2024-official.json new file: src/main/deploy/apriltags/2024-speakers.json new file: src/main/deploy/apriltags/2024-wpi.json modified: src/main/deploy/swerve/modules/backleft.json modified: src/main/deploy/swerve/modules/backright.json modified: src/main/deploy/swerve/modules/frontleft.json modified: src/main/deploy/swerve/modules/frontright.json modified: src/main/deploy/swerve/swervedrive.json modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java new file: src/main/java/frc/robot/util/YagslConstants.java --- src/main/deploy/apriltags/2024-amps.json | 44 +++ src/main/deploy/apriltags/2024-official.json | 296 ++++++++++++++++++ src/main/deploy/apriltags/2024-speakers.json | 80 +++++ src/main/deploy/apriltags/2024-wpi.json | 116 +++++++ src/main/deploy/swerve/modules/backleft.json | 6 +- src/main/deploy/swerve/modules/backright.json | 6 +- src/main/deploy/swerve/modules/frontleft.json | 6 +- .../deploy/swerve/modules/frontright.json | 6 +- src/main/deploy/swerve/swervedrive.json | 2 +- src/main/java/frc/robot/Constants.java | 51 +-- .../accelerometer/Accelerometer.java | 3 +- .../java/frc/robot/util/YagslConstants.java | 187 +++++++++++ 12 files changed, 753 insertions(+), 50 deletions(-) create mode 100644 src/main/deploy/apriltags/2024-amps.json create mode 100644 src/main/deploy/apriltags/2024-official.json create mode 100644 src/main/deploy/apriltags/2024-speakers.json create mode 100644 src/main/deploy/apriltags/2024-wpi.json create mode 100644 src/main/java/frc/robot/util/YagslConstants.java diff --git a/src/main/deploy/apriltags/2024-amps.json b/src/main/deploy/apriltags/2024-amps.json new file mode 100644 index 00000000..ddaf4be3 --- /dev/null +++ b/src/main/deploy/apriltags/2024-amps.json @@ -0,0 +1,44 @@ +{ + "tags": [ + { + "ID": 5, + "pose": { + "translation": { + "x": 14.700757999999999, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 1.8415, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.211 + } +} diff --git a/src/main/deploy/apriltags/2024-official.json b/src/main/deploy/apriltags/2024-official.json new file mode 100644 index 00000000..8cb837a5 --- /dev/null +++ b/src/main/deploy/apriltags/2024-official.json @@ -0,0 +1,296 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 15.079471999999997, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 16.185134, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 16.579342, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 16.579342, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 14.700757999999999, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 1.8415, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 0.356108, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 1.4615159999999998, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 11.904726, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.904726, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 11.220196, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 5.320792, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 4.641342, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 4.641342, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.211 + } +} diff --git a/src/main/deploy/apriltags/2024-speakers.json b/src/main/deploy/apriltags/2024-speakers.json new file mode 100644 index 00000000..36ecc9df --- /dev/null +++ b/src/main/deploy/apriltags/2024-speakers.json @@ -0,0 +1,80 @@ +{ + "tags": [ + { + "ID": 3, + "pose": { + "translation": { + "x": 16.579342, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 16.579342, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.211 + } +} diff --git a/src/main/deploy/apriltags/2024-wpi.json b/src/main/deploy/apriltags/2024-wpi.json new file mode 100644 index 00000000..bbf6862c --- /dev/null +++ b/src/main/deploy/apriltags/2024-wpi.json @@ -0,0 +1,116 @@ +{ + "tags": [ + { + "ID": 3, + "pose": { + "translation": { + "x": 16.579342, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 16.579342, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 14.700757999999999, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 1.8415, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.211 + } +} diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json index 614c45a4..08f078a0 100644 --- a/src/main/deploy/swerve/modules/backleft.json +++ b/src/main/deploy/swerve/modules/backleft.json @@ -7,7 +7,7 @@ "drive": { "type": "falcon", "id": 7, - "canbus": "CANivore" + "canbus": "DriveTrain" }, "inverted": { "drive": true, @@ -16,12 +16,12 @@ "angle": { "type": "falcon", "id": 8, - "canbus": "CANivore" + "canbus": "DriveTrain" }, "encoder": { "type": "cancoder", "id": 9, - "canbus": "CANivore" + "canbus": "DriveTrain" }, "absoluteEncoderInverted": false } diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json index 7a8eee6f..d48c3768 100644 --- a/src/main/deploy/swerve/modules/backright.json +++ b/src/main/deploy/swerve/modules/backright.json @@ -7,7 +7,7 @@ "drive": { "type": "falcon", "id": 10, - "canbus": "CANivore" + "canbus": "DriveTrain" }, "inverted": { "drive": true, @@ -16,12 +16,12 @@ "angle": { "type": "falcon", "id": 11, - "canbus": "CANivore" + "canbus": "DriveTrain" }, "encoder": { "type": "cancoder", "id": 12, - "canbus": "CANivore" + "canbus": "DriveTrain" }, "absoluteEncoderInverted": false } diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json index d7e7cbcc..54f9bcf5 100644 --- a/src/main/deploy/swerve/modules/frontleft.json +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -7,7 +7,7 @@ "drive": { "type": "falcon", "id": 1, - "canbus": "CANivore" + "canbus": "DriveTrain" }, "inverted": { "drive": true, @@ -16,12 +16,12 @@ "angle": { "type": "falcon", "id": 2, - "canbus": "CANivore" + "canbus": "DriveTrain" }, "encoder": { "type": "cancoder", "id": 3, - "canbus": "CANivore" + "canbus": "DriveTrain" }, "absoluteEncoderInverted": false } diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json index e7f49d41..d1de2ab9 100644 --- a/src/main/deploy/swerve/modules/frontright.json +++ b/src/main/deploy/swerve/modules/frontright.json @@ -7,7 +7,7 @@ "drive": { "type": "falcon", "id": 4, - "canbus": "CANivore" + "canbus": "DriveTrain" }, "inverted": { "drive": true, @@ -16,12 +16,12 @@ "angle": { "type": "falcon", "id": 5, - "canbus": "CANivore" + "canbus": "DriveTrain" }, "encoder": { "type": "cancoder", "id": 6, - "canbus": "CANivore" + "canbus": "DriveTrain" }, "absoluteEncoderInverted": false } diff --git a/src/main/deploy/swerve/swervedrive.json b/src/main/deploy/swerve/swervedrive.json index 2232e85f..26406d90 100644 --- a/src/main/deploy/swerve/swervedrive.json +++ b/src/main/deploy/swerve/swervedrive.json @@ -2,7 +2,7 @@ "imu": { "type": "pigeon2", "id": 13, - "canbus": "CANivore" + "canbus": "DriveTrain" }, "invertedIMU": false, "modules": [ diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 812c4c6c..728da6fb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -32,12 +32,10 @@ import frc.robot.generated.TunerConstants; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; -import java.io.File; import java.io.IOException; import java.nio.file.Path; import lombok.Getter; import swervelib.math.Matter; -import swervelib.parser.json.SwerveDriveJson; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -118,24 +116,6 @@ public static SwerveType getSwerveType() { return swerveType; } - /** Location of YAGSL JSON files (if using) */ - public static class YagslConstants { - public static final File yagslDir = new File(Filesystem.getDeployDirectory(), "swerve"); - public static final SwerveDriveJson swerveDriveJson; - - static { - SwerveDriveJson tempJson = null; - try { - tempJson = - new ObjectMapper() - .readValue(new File(yagslDir, "swervedrive.json"), SwerveDriveJson.class); - } catch (Exception e) { - throw new RuntimeException(e); - } - swerveDriveJson = tempJson; - } - } - /** Enumerate the supported autonomous path planning types */ public static enum AutoType { PATHPLANNER, // PathPlanner (https://pathplanner.dev/home.html) @@ -184,6 +164,14 @@ public static final class PhysicalConstants { public static final double kLoopTime = 0.13; // s, 20ms + 110ms sprk max velocity lag } + /** Deploy Directoy Location Constants *********************************** */ + public static final class DeployConstants { + public static final String apriltagDir = "apriltags"; + public static final String choreoDir = "choreo"; + public static final String pathplannerDir = "pathplanner"; + public static final String yagslDir = "swerve"; + } + /** Power Distribution Constants ********************************** */ public static final class PowerConstants { @@ -225,29 +213,20 @@ public static final class AutoConstants { /** Drive Base Constants ************************************************* */ public static final class DrivebaseConstants { - // Physical size of the drive base - private static final double kTrackWidthX = Units.inchesToMeters(20.75); - private static final double kTrackWidthY = Units.inchesToMeters(20.75); - public static final double kDriveBaseRadius = - Math.hypot(kTrackWidthX / 2.0, kTrackWidthY / 2.0); - - // CTRE-based maximum robot speeds (adjustable in Phoenix X Tuner Swerve Generator) - // kSpeedAt12VoltsMps desired top speed + // Theoretical free speed (m/s) at 12v applied output; + // This needs to be tuned to your individual robot + // NOTE: If using SwerveType.PHOENIX6, adjust this in the Phoenix X Tuner Swerve Generator public static final double kMaxLinearSpeed = TunerConstants.kSpeedAt12VoltsMps; - // 3/4 of a rotation per second max angular velocity + // Otherwise, set the maximum linear speed here + // public static final double kMaxLinearSpeed = 5.21; + + // Set 3/4 of a rotation per second as the max angular velocity public static final double kMaxAngularSpeed = 1.5 * Math.PI; // Maximum chassis accelerations desired for robot motion -- metric / radians public static final double kMaxLinearAccel = 4.0; // m/s/s public static final double kMaxAngularAccel = Units.degreesToRadians(720); // deg/s/s - // Wheel radius - public static final double kWheelRadius = Units.inchesToMeters(2.0); - - // ** Gear ratios for SDS MK4i L2, adjust as necessary ** - public static final double kDriveGearRatio = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - public static final double kTurnGearRatio = 150.0 / 7.0; - // Hold time on motor brakes when disabled public static final double kWheelLockTime = 10; // seconds diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 5b611b45..63356c7f 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -22,8 +22,8 @@ import edu.wpi.first.wpilibj.BuiltInAccelerometer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; -import frc.robot.Constants.YagslConstants; import frc.robot.util.VirtualSubsystem; +import frc.robot.util.YagslConstants; import org.littletonrobotics.junction.Logger; /** @@ -84,6 +84,7 @@ public Accelerometer(Object accelerometer) { /** Get accelerations, compute jerks, log everything */ public void periodic() { + // Log the execution time long start = System.nanoTime(); // Compute the Rio's acceleration, rotated as needed in the XY plane (yaw) diff --git a/src/main/java/frc/robot/util/YagslConstants.java b/src/main/java/frc/robot/util/YagslConstants.java new file mode 100644 index 00000000..3bc728e9 --- /dev/null +++ b/src/main/java/frc/robot/util/YagslConstants.java @@ -0,0 +1,187 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.util; + +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.wpilibj.Filesystem; +import frc.robot.Constants.DeployConstants; +import java.io.File; +import java.util.Arrays; +import java.util.List; +import swervelib.parser.json.ModuleJson; +import swervelib.parser.json.PIDFPropertiesJson; +import swervelib.parser.json.PhysicalPropertiesJson; +import swervelib.parser.json.SwerveDriveJson; + +/** + * The YAGSL swerve drive constants class + * + *

The purpose of this class is to convert the YAGSL-based JSON configuration file data into the + * same constants format as produced by the Phoenix Tuner X's ``TunerConstants.java`` file. The goal + * here is to make implenentation in the drive class easier. + */ +public class YagslConstants { + + // Define the internal YAGSL objects we will read into + private static final File yagslDir = + new File(Filesystem.getDeployDirectory(), DeployConstants.yagslDir); + public static final SwerveDriveJson swerveDriveJson; // Needed by the Accelerometer subsystem + private static final PIDFPropertiesJson pidfPropertiesJson; + private static final PhysicalPropertiesJson physicalPropertiesJson; + private static final ModuleJson[] moduleJsons; + + // Use a static to read in the YAGSL JSON files with error catching + static { + SwerveDriveJson tempSwerveJson = null; + PIDFPropertiesJson tempPdifJson = null; + PhysicalPropertiesJson tempPhysicalJson = null; + ModuleJson[] tempModuleJsons = null; + + try { + tempSwerveJson = + new ObjectMapper() + .readValue(new File(yagslDir, "swervedrive.json"), SwerveDriveJson.class); + tempPdifJson = + new ObjectMapper() + .readValue( + new File(yagslDir, "modules/pidfproperties.json"), PIDFPropertiesJson.class); + tempPhysicalJson = + new ObjectMapper() + .readValue( + new File(yagslDir, "modules/physicalproperties.json"), + PhysicalPropertiesJson.class); + + } catch (Exception e) { + throw new RuntimeException(e); + } + swerveDriveJson = tempSwerveJson; + pidfPropertiesJson = tempPdifJson; + physicalPropertiesJson = tempPhysicalJson; + + tempModuleJsons = new ModuleJson[swerveDriveJson.modules.length]; + try { + for (int i = 0; i < tempModuleJsons.length; i++) { + // moduleConfigs.put(swerveDriveJson.modules[i], i); + File moduleFile = new File(yagslDir, "modules/" + swerveDriveJson.modules[i]); + assert moduleFile.exists(); + tempModuleJsons[i] = new ObjectMapper().readValue(moduleFile, ModuleJson.class); + } + } catch (Exception e) { + throw new RuntimeException(e); + } + moduleJsons = tempModuleJsons; + } + + // Define the Tuner X - like constants from the YAGSL objects + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + public static final double kCoupleRatio = 3.5; + + public static final double kDriveGearRatio = + physicalPropertiesJson.conversionFactors.drive.gearRatio; + public static final double kSteerGearRatio = + physicalPropertiesJson.conversionFactors.angle.gearRatio; + public static final double kWheelRadiusInches = + physicalPropertiesJson.conversionFactors.drive.diameter / 2.0; + + public static final String kCANbusName = swerveDriveJson.imu.canbus; + public static final int kPigeonId = swerveDriveJson.imu.id; + + // These are only used for simulation + public static final double kSteerInertia = 0.004; + public static final double kDriveInertia = 0.025; + // Simulated voltage necessary to overcome friction + public static final double kSteerFrictionVoltage = 0.25; + public static final double kDriveFrictionVoltage = 0.25; + + // The modules themselves + static List moduleList = Arrays.asList(swerveDriveJson.modules); + + // Front Left + static ModuleJson flModule = moduleJsons[moduleList.indexOf("frontleft.json")]; + public static final int kFrontLeftDriveMotorId = flModule.drive.id; + public static final int kFrontLeftSteerMotorId = flModule.angle.id; + public static final int kFrontLeftEncoderId = flModule.encoder.id; + public static final String kFrontLeftDriveCanbus = flModule.drive.canbus; + public static final String kFrontLeftSteerCanbus = flModule.angle.canbus; + public static final String kFrontLeftEncoderCanbus = flModule.encoder.canbus; + public static final double kFrontLeftEncoderOffset = flModule.absoluteEncoderOffset; + public static final boolean kFrontLeftDriveInvert = flModule.inverted.drive; + public static final boolean kFrontLeftSteerInvert = flModule.inverted.angle; + public static final boolean kFrontLeftEncoderInvert = flModule.absoluteEncoderInverted; + public static final double kFrontLeftXPosInches = flModule.location.left; + public static final double kFrontLeftYPosInches = flModule.location.front; + + // Front Right + static ModuleJson frModule = moduleJsons[moduleList.indexOf("frontright.json")]; + public static final int kFrontRightDriveMotorId = frModule.drive.id; + public static final int kFrontRightSteerMotorId = frModule.angle.id; + public static final int kFrontRightEncoderId = frModule.encoder.id; + public static final String kFrontRightDriveCanbus = frModule.drive.canbus; + public static final String kFrontRightSteerCanbus = frModule.angle.canbus; + public static final String kFrontRightEncoderCanbus = frModule.encoder.canbus; + public static final double kFrontRightEncoderOffset = frModule.absoluteEncoderOffset; + public static final boolean KFrontRightDriveInvert = frModule.inverted.drive; + public static final boolean KFrontRightSteerInvert = frModule.inverted.angle; + public static final boolean KFrontRightEncoderInvert = frModule.absoluteEncoderInverted; + public static final double kFrontRightXPosInches = frModule.location.left; + public static final double kFrontRightYPosInches = frModule.location.front; + + // Back Left + static ModuleJson blModule = moduleJsons[moduleList.indexOf("backleft.json")]; + public static final int kBackLeftDriveMotorId = blModule.drive.id; + public static final int kBackLeftSteerMotorId = blModule.angle.id; + public static final int kBackLeftEncoderId = blModule.encoder.id; + public static final String kBackLeftDriveCanbus = blModule.drive.canbus; + public static final String kBackLeftSteerCanbus = blModule.angle.canbus; + public static final String kBackLeftEncoderCanbus = blModule.encoder.canbus; + public static final double kBackLeftEncoderOffset = blModule.absoluteEncoderOffset; + public static final boolean kBackLeftDriveInvert = blModule.inverted.drive; + public static final boolean kBackLeftSteerInvert = blModule.inverted.angle; + public static final boolean kBackLeftEncoderInvert = blModule.absoluteEncoderInverted; + public static final double kBackLeftXPosInches = blModule.location.left; + public static final double kBackLeftYPosInches = blModule.location.front; + + // Back Right + static ModuleJson brModule = moduleJsons[moduleList.indexOf("backright.json")]; + public static final int kBackRightDriveMotorId = brModule.drive.id; + public static final int kBackRightSteerMotorId = brModule.angle.id; + public static final int kBackRightEncoderId = brModule.encoder.id; + public static final String kBackRightDriveCanbus = brModule.drive.canbus; + public static final String kBackRightSteerCanbus = brModule.angle.canbus; + public static final String kBackRightEncoderCanbus = brModule.encoder.canbus; + public static final double kBackRightEncoderOffset = brModule.absoluteEncoderOffset; + public static final boolean KBackRightDriveInvert = brModule.inverted.drive; + public static final boolean KBackRightSteerInvert = brModule.inverted.angle; + public static final boolean KBackRightEncoderInvert = brModule.absoluteEncoderInverted; + public static final double kBackRightXPosInches = brModule.location.left; + public static final double kBackRightYPosInches = brModule.location.front; + + // YAGSL PIDF Values + public static final double kDriveP = pidfPropertiesJson.drive.p; + public static final double kDriveI = pidfPropertiesJson.drive.i; + public static final double kDriveD = pidfPropertiesJson.drive.d; + public static final double kDriveF = pidfPropertiesJson.drive.f; + public static final double kDriveIZ = pidfPropertiesJson.drive.iz; + public static final double kSteerP = pidfPropertiesJson.angle.p; + public static final double kSteerI = pidfPropertiesJson.angle.i; + public static final double kSteerD = pidfPropertiesJson.angle.d; + public static final double kSteerF = pidfPropertiesJson.angle.f; + public static final double kSteerIZ = pidfPropertiesJson.angle.iz; +} From 23a59ad8d8d00920ee3f4b3f66680040ea0b3da3 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 22 Nov 2024 13:56:05 -0700 Subject: [PATCH 48/57] Drivebase constants from Phoenix / YAGSL combined Drive subsystem now has access to unified constant values from either input swerve framework. Next step is to construct the drive object and try to make everything happy. modified: src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java modified: src/main/java/frc/robot/subsystems/drive/Drive.java new file: src/main/java/frc/robot/subsystems/drive/DriveConstants.java new file: src/main/java/frc/robot/subsystems/drive/GyroIONavX.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java modified: src/main/java/frc/robot/util/YagslConstants.java --- .../accelerometer/Accelerometer.java | 1 - .../frc/robot/subsystems/drive/Drive.java | 40 +-- .../subsystems/drive/DriveConstants.java | 314 ++++++++++++++++++ .../robot/subsystems/drive/GyroIONavX.java | 51 +++ .../subsystems/drive/ModuleIOTalonFX.java | 2 +- .../java/frc/robot/util/YagslConstants.java | 6 +- 6 files changed, 379 insertions(+), 35 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveConstants.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIONavX.java diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 63356c7f..91519de3 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -75,7 +75,6 @@ public Accelerometer(Object accelerometer) { default: // Otherwise kick a message to the console, and set to null System.out.println("WARNING: Cannot initialize IMU's accelerometer for logging!"); - ; this.pigeonAccelerometer = null; this.navXAccelerometer = null; } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index dbf437b5..5105072a 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -31,25 +31,18 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; -import frc.robot.generated.TunerConstants; +import frc.robot.Constants.DrivebaseConstants; import frc.robot.util.LocalADStarAK; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Drive extends SubsystemBase { - private static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5); - private static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); - private static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); - private static final double DRIVE_BASE_RADIUS = - Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); - private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); @@ -72,7 +65,7 @@ public class Drive extends SubsystemBase { public Drive(Constants.SwerveType swerveType) { switch (swerveType) { case PHOENIX6: - gyroIO = new GyroIOPigeon2(TunerConstants.kPigeonId, TunerConstants.kCANbusName); + gyroIO = new GyroIOPigeon2(DriveConstants.kPigeonId, DriveConstants.kCANbusName); modules[0] = new Module(new ModuleIOTalonFX(0), 0); modules[0] = new Module(new ModuleIOTalonFX(0), 0); modules[0] = new Module(new ModuleIOTalonFX(0), 0); @@ -104,7 +97,7 @@ public Drive( () -> kinematics.toChassisSpeeds(getModuleStates()), this::runVelocity, new HolonomicPathFollowerConfig( - MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), + DrivebaseConstants.kMaxLinearSpeed, DRIVE_BASE_RADIUS, new ReplanningConfig()), () -> DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red, @@ -192,7 +185,7 @@ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DrivebaseConstants.kMaxLinearSpeed); // Send setpoints to modules SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; @@ -281,38 +274,25 @@ public void addVisionMeasurement(Pose2d visionPose, double timestamp) { /** Returns the maximum linear speed in meters per sec. */ public double getMaxLinearSpeedMetersPerSec() { - return MAX_LINEAR_SPEED; + return DrivebaseConstants.kMaxLinearSpeed; } /** Returns the maximum angular speed in radians per sec. */ public double getMaxAngularSpeedRadPerSec() { - return MAX_ANGULAR_SPEED; + return DrivebaseConstants.kMaxAngularSpeed; } /** Returns an array of module translations. */ public static Translation2d[] getModuleTranslations() { return new Translation2d[] { - new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), - new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0), - new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), - new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0) + new Translation2d(DriveConstants.kFrontLeftXPosInches, DriveConstants.kFrontLeftYPosInches), + new Translation2d(DriveConstants.kFrontRightXPosInches, DriveConstants.kFrontRightYPosInches), + new Translation2d(DriveConstants.kBackLeftXPosInches, DriveConstants.kBackLeftYPosInches), + new Translation2d(DriveConstants.kBackRightXPosInches, DriveConstants.kBackRightYPosInches) }; } public T getGyro() { return gyroIO.getGyro(); } - - public double getDriveGearRatio() { - switch (Constants.getSwerveType()) { - case PHOENIX6: - return TunerConstants.kDriveGearRatio; - case YAGSL: - return; - } - } - - public double getTurnGearRatio() { - return 0.0; - } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java new file mode 100644 index 00000000..1ae0f145 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -0,0 +1,314 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import frc.robot.Constants; +import frc.robot.generated.TunerConstants; +import frc.robot.util.YagslConstants; + +/** + * Holds the proper set of drive constants given the type of drive + * + *

Does some translation of how the two keep values to return a completely unified API. This file + * should not be modified. + */ +public class DriveConstants { + + // Declare all the constants + public static final double kCoupleRatio; + public static final double kDriveGearRatio; + public static final double kSteerGearRatio; + public static final double kWheelRadiusInches; + public static final String kCANbusName; + public static final int kPigeonId; + public static final double kSteerInertia; + public static final double kDriveInertia; + public static final double kSteerFrictionVoltage; + public static final double kDriveFrictionVoltage; + public static final int kFrontLeftDriveMotorId; + public static final int kFrontLeftSteerMotorId; + public static final int kFrontLeftEncoderId; + public static final String kFrontLeftDriveCanbus; + public static final String kFrontLeftSteerCanbus; + public static final String kFrontLeftEncoderCanbus; + public static final double kFrontLeftEncoderOffset; + public static final boolean kFrontLeftDriveInvert; + public static final boolean kFrontLeftSteerInvert; + public static final boolean kFrontLeftEncoderInvert; + public static final double kFrontLeftXPosInches; + public static final double kFrontLeftYPosInches; + public static final int kFrontRightDriveMotorId; + public static final int kFrontRightSteerMotorId; + public static final int kFrontRightEncoderId; + public static final String kFrontRightDriveCanbus; + public static final String kFrontRightSteerCanbus; + public static final String kFrontRightEncoderCanbus; + public static final double kFrontRightEncoderOffset; + public static final boolean KFrontRightDriveInvert; + public static final boolean KFrontRightSteerInvert; + public static final boolean KFrontRightEncoderInvert; + public static final double kFrontRightXPosInches; + public static final double kFrontRightYPosInches; + public static final int kBackLeftDriveMotorId; + public static final int kBackLeftSteerMotorId; + public static final int kBackLeftEncoderId; + public static final String kBackLeftDriveCanbus; + public static final String kBackLeftSteerCanbus; + public static final String kBackLeftEncoderCanbus; + public static final double kBackLeftEncoderOffset; + public static final boolean kBackLeftDriveInvert; + public static final boolean kBackLeftSteerInvert; + public static final boolean kBackLeftEncoderInvert; + public static final double kBackLeftXPosInches; + public static final double kBackLeftYPosInches; + public static final int kBackRightDriveMotorId; + public static final int kBackRightSteerMotorId; + public static final int kBackRightEncoderId; + public static final String kBackRightDriveCanbus; + public static final String kBackRightSteerCanbus; + public static final String kBackRightEncoderCanbus; + public static final double kBackRightEncoderOffset; + public static final boolean kBackRightDriveInvert; + public static final boolean kBackRightSteerInvert; + public static final boolean kBackRightEncoderInvert; + public static final double kBackRightXPosInches; + public static final double kBackRightYPosInches; + public static final double kDriveP; + public static final double kDriveI; + public static final double kDriveD; + public static final double kDriveF; + public static final double kDriveIZ; + public static final double kSteerP; + public static final double kSteerI; + public static final double kSteerD; + public static final double kSteerF; + public static final double kSteerIZ; + + // Fill in the values from the proper source + static { + switch (Constants.getSwerveType()) { + case PHOENIX6: + kCoupleRatio = TunerConstants.kCoupleRatio; + kDriveGearRatio = TunerConstants.kDriveGearRatio; + kSteerGearRatio = TunerConstants.kSteerGearRatio; + kWheelRadiusInches = TunerConstants.kWheelRadiusInches; + kCANbusName = TunerConstants.kCANbusName; + kPigeonId = TunerConstants.kPigeonId; + kSteerInertia = TunerConstants.kSteerInertia; + kDriveInertia = TunerConstants.kDriveInertia; + kSteerFrictionVoltage = TunerConstants.kSteerFrictionVoltage; + kDriveFrictionVoltage = TunerConstants.kDriveFrictionVoltage; + kFrontLeftDriveMotorId = TunerConstants.kFrontLeftDriveMotorId; + kFrontLeftSteerMotorId = TunerConstants.kFrontLeftSteerMotorId; + kFrontLeftEncoderId = TunerConstants.kFrontLeftEncoderId; + kFrontLeftDriveCanbus = TunerConstants.kCANbusName; + kFrontLeftSteerCanbus = TunerConstants.kCANbusName; + kFrontLeftEncoderCanbus = TunerConstants.kCANbusName; + kFrontLeftEncoderOffset = TunerConstants.kFrontLeftEncoderOffset; + kFrontLeftDriveInvert = TunerConstants.kInvertLeftSide; + kFrontLeftSteerInvert = TunerConstants.kFrontLeftSteerInvert; + kFrontLeftEncoderInvert = false; + kFrontLeftXPosInches = TunerConstants.kFrontLeftXPosInches; + kFrontLeftYPosInches = TunerConstants.kFrontLeftYPosInches; + kFrontRightDriveMotorId = TunerConstants.kFrontRightDriveMotorId; + kFrontRightSteerMotorId = TunerConstants.kFrontRightSteerMotorId; + kFrontRightEncoderId = TunerConstants.kFrontRightEncoderId; + kFrontRightDriveCanbus = TunerConstants.kCANbusName; + kFrontRightSteerCanbus = TunerConstants.kCANbusName; + kFrontRightEncoderCanbus = TunerConstants.kCANbusName; + kFrontRightEncoderOffset = TunerConstants.kFrontRightEncoderOffset; + KFrontRightDriveInvert = TunerConstants.kInvertRightSide; + KFrontRightSteerInvert = TunerConstants.kFrontRightSteerInvert; + KFrontRightEncoderInvert = false; + kFrontRightXPosInches = TunerConstants.kFrontRightXPosInches; + kFrontRightYPosInches = TunerConstants.kFrontRightYPosInches; + kBackLeftDriveMotorId = TunerConstants.kBackLeftDriveMotorId; + kBackLeftSteerMotorId = TunerConstants.kBackLeftSteerMotorId; + kBackLeftEncoderId = TunerConstants.kBackLeftEncoderId; + kBackLeftDriveCanbus = TunerConstants.kCANbusName; + kBackLeftSteerCanbus = TunerConstants.kCANbusName; + kBackLeftEncoderCanbus = TunerConstants.kCANbusName; + kBackLeftEncoderOffset = TunerConstants.kBackLeftEncoderOffset; + kBackLeftDriveInvert = TunerConstants.kInvertLeftSide; + kBackLeftSteerInvert = TunerConstants.kBackLeftSteerInvert; + kBackLeftEncoderInvert = false; + kBackLeftXPosInches = TunerConstants.kBackLeftXPosInches; + kBackLeftYPosInches = TunerConstants.kBackLeftYPosInches; + kBackRightDriveMotorId = TunerConstants.kBackRightDriveMotorId; + kBackRightSteerMotorId = TunerConstants.kBackRightSteerMotorId; + kBackRightEncoderId = TunerConstants.kBackRightEncoderId; + kBackRightDriveCanbus = TunerConstants.kCANbusName; + kBackRightSteerCanbus = TunerConstants.kCANbusName; + kBackRightEncoderCanbus = TunerConstants.kCANbusName; + kBackRightEncoderOffset = TunerConstants.kBackRightEncoderOffset; + kBackRightDriveInvert = TunerConstants.kInvertRightSide; + kBackRightSteerInvert = TunerConstants.kBackRightSteerInvert; + kBackRightEncoderInvert = false; + kBackRightXPosInches = TunerConstants.kBackRightXPosInches; + kBackRightYPosInches = TunerConstants.kBackRightYPosInches; + kDriveP = TunerConstants.driveGains.kP; + kDriveI = TunerConstants.driveGains.kI; + kDriveD = TunerConstants.driveGains.kD; + kDriveF = TunerConstants.driveGains.kV; + kDriveIZ = 0.0; + kSteerP = TunerConstants.steerGains.kP; + kSteerI = TunerConstants.steerGains.kI; + kSteerD = TunerConstants.steerGains.kD; + kSteerF = TunerConstants.steerGains.kV; + kSteerIZ = 0.0; + break; + + case YAGSL: + kCoupleRatio = YagslConstants.kCoupleRatio; + kDriveGearRatio = YagslConstants.kDriveGearRatio; + kSteerGearRatio = YagslConstants.kSteerGearRatio; + kWheelRadiusInches = YagslConstants.kWheelRadiusInches; + kCANbusName = YagslConstants.kCANbusName; + kPigeonId = YagslConstants.kPigeonId; + kSteerInertia = YagslConstants.kSteerInertia; + kDriveInertia = YagslConstants.kDriveInertia; + kSteerFrictionVoltage = YagslConstants.kSteerFrictionVoltage; + kDriveFrictionVoltage = YagslConstants.kDriveFrictionVoltage; + kFrontLeftDriveMotorId = YagslConstants.kFrontLeftDriveMotorId; + kFrontLeftSteerMotorId = YagslConstants.kFrontLeftSteerMotorId; + kFrontLeftEncoderId = YagslConstants.kFrontLeftEncoderId; + kFrontLeftDriveCanbus = YagslConstants.kFrontLeftDriveCanbus; + kFrontLeftSteerCanbus = YagslConstants.kFrontLeftSteerCanbus; + kFrontLeftEncoderCanbus = YagslConstants.kFrontLeftEncoderCanbus; + kFrontLeftEncoderOffset = YagslConstants.kFrontLeftEncoderOffset; + kFrontLeftDriveInvert = YagslConstants.kFrontLeftDriveInvert; + kFrontLeftSteerInvert = YagslConstants.kFrontLeftSteerInvert; + kFrontLeftEncoderInvert = YagslConstants.kFrontLeftEncoderInvert; + kFrontLeftXPosInches = YagslConstants.kFrontLeftXPosInches; + kFrontLeftYPosInches = YagslConstants.kFrontLeftYPosInches; + kFrontRightDriveMotorId = YagslConstants.kFrontRightDriveMotorId; + kFrontRightSteerMotorId = YagslConstants.kFrontRightSteerMotorId; + kFrontRightEncoderId = YagslConstants.kFrontRightEncoderId; + kFrontRightDriveCanbus = YagslConstants.kFrontRightDriveCanbus; + kFrontRightSteerCanbus = YagslConstants.kFrontRightSteerCanbus; + kFrontRightEncoderCanbus = YagslConstants.kFrontRightEncoderCanbus; + kFrontRightEncoderOffset = YagslConstants.kFrontRightEncoderOffset; + KFrontRightDriveInvert = YagslConstants.KFrontRightDriveInvert; + KFrontRightSteerInvert = YagslConstants.KFrontRightSteerInvert; + KFrontRightEncoderInvert = YagslConstants.KFrontRightEncoderInvert; + kFrontRightXPosInches = YagslConstants.kFrontRightXPosInches; + kFrontRightYPosInches = YagslConstants.kFrontRightYPosInches; + kBackLeftDriveMotorId = YagslConstants.kBackLeftDriveMotorId; + kBackLeftSteerMotorId = YagslConstants.kBackLeftSteerMotorId; + kBackLeftEncoderId = YagslConstants.kBackLeftEncoderId; + kBackLeftDriveCanbus = YagslConstants.kBackLeftDriveCanbus; + kBackLeftSteerCanbus = YagslConstants.kBackLeftSteerCanbus; + kBackLeftEncoderCanbus = YagslConstants.kBackLeftEncoderCanbus; + kBackLeftEncoderOffset = YagslConstants.kBackLeftEncoderOffset; + kBackLeftDriveInvert = YagslConstants.kBackLeftDriveInvert; + kBackLeftSteerInvert = YagslConstants.kBackLeftSteerInvert; + kBackLeftEncoderInvert = YagslConstants.kBackLeftEncoderInvert; + kBackLeftXPosInches = YagslConstants.kBackLeftXPosInches; + kBackLeftYPosInches = YagslConstants.kBackLeftYPosInches; + kBackRightDriveMotorId = YagslConstants.kBackRightDriveMotorId; + kBackRightSteerMotorId = YagslConstants.kBackRightSteerMotorId; + kBackRightEncoderId = YagslConstants.kBackRightEncoderId; + kBackRightDriveCanbus = YagslConstants.kBackRightDriveCanbus; + kBackRightSteerCanbus = YagslConstants.kBackRightSteerCanbus; + kBackRightEncoderCanbus = YagslConstants.kBackRightEncoderCanbus; + kBackRightEncoderOffset = YagslConstants.kBackRightEncoderOffset; + kBackRightDriveInvert = YagslConstants.kBackRightDriveInvert; + kBackRightSteerInvert = YagslConstants.kBackRightSteerInvert; + kBackRightEncoderInvert = YagslConstants.kBackRightEncoderInvert; + kBackRightXPosInches = YagslConstants.kBackRightXPosInches; + kBackRightYPosInches = YagslConstants.kBackRightYPosInches; + kDriveP = YagslConstants.kDriveP; + kDriveI = YagslConstants.kDriveI; + kDriveD = YagslConstants.kDriveD; + kDriveF = YagslConstants.kDriveF; + kDriveIZ = YagslConstants.kDriveIZ; + kSteerP = YagslConstants.kSteerP; + kSteerI = YagslConstants.kSteerI; + kSteerD = YagslConstants.kSteerD; + kSteerF = YagslConstants.kSteerF; + kSteerIZ = YagslConstants.kSteerIZ; + break; + + default: + kCoupleRatio = 0.0; + kDriveGearRatio = 0.0; + kSteerGearRatio = 0.0; + kWheelRadiusInches = 0.0; + kCANbusName = ""; + kPigeonId = 0; + kSteerInertia = 0.0; + kDriveInertia = 0.0; + kSteerFrictionVoltage = 0.0; + kDriveFrictionVoltage = 0.0; + kFrontLeftDriveMotorId = 0; + kFrontLeftSteerMotorId = 0; + kFrontLeftEncoderId = 0; + kFrontLeftDriveCanbus = ""; + kFrontLeftSteerCanbus = ""; + kFrontLeftEncoderCanbus = ""; + kFrontLeftEncoderOffset = 0.0; + kFrontLeftDriveInvert = false; + kFrontLeftSteerInvert = false; + kFrontLeftEncoderInvert = false; + kFrontLeftXPosInches = 0.0; + kFrontLeftYPosInches = 0.0; + kFrontRightDriveMotorId = 0; + kFrontRightSteerMotorId = 0; + kFrontRightEncoderId = 0; + kFrontRightDriveCanbus = ""; + kFrontRightSteerCanbus = ""; + kFrontRightEncoderCanbus = ""; + kFrontRightEncoderOffset = 0.0; + KFrontRightDriveInvert = false; + KFrontRightSteerInvert = false; + KFrontRightEncoderInvert = false; + kFrontRightXPosInches = 0.0; + kFrontRightYPosInches = 0.0; + kBackLeftDriveMotorId = 0; + kBackLeftSteerMotorId = 0; + kBackLeftEncoderId = 0; + kBackLeftDriveCanbus = ""; + kBackLeftSteerCanbus = ""; + kBackLeftEncoderCanbus = ""; + kBackLeftEncoderOffset = 0.0; + kBackLeftDriveInvert = false; + kBackLeftSteerInvert = false; + kBackLeftEncoderInvert = false; + kBackLeftXPosInches = 0.0; + kBackLeftYPosInches = 0.0; + kBackRightDriveMotorId = 0; + kBackRightSteerMotorId = 0; + kBackRightEncoderId = 0; + kBackRightDriveCanbus = ""; + kBackRightSteerCanbus = ""; + kBackRightEncoderCanbus = ""; + kBackRightEncoderOffset = 0.0; + kBackRightDriveInvert = false; + kBackRightSteerInvert = false; + kBackRightEncoderInvert = false; + kBackRightXPosInches = 0.0; + kBackRightYPosInches = 0.0; + kDriveP = 0.0; + kDriveI = 0.0; + kDriveD = 0.0; + kDriveF = 0.0; + kDriveIZ = 0.0; + kSteerP = 0.0; + kSteerI = 0.0; + kSteerD = 0.0; + kSteerF = 0.0; + kSteerIZ = 0.0; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java new file mode 100644 index 00000000..ca20381d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -0,0 +1,51 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.SPI; + +/** IO implementation for Pigeon2 */ +public class GyroIONavX implements GyroIO { + private final AHRS navx; + + // Constructor, taking default values + public GyroIONavX() { + navx = new AHRS(SPI.Port.kMXP, (byte) 100.0); + navx.reset(); + } + + // Constructor, taking deviceID and canBus (which are ignored, since it is connected via SPI) + public GyroIONavX(int deviceID, String canBus) { + navx = new AHRS(SPI.Port.kMXP, (byte) 100.0); + navx.reset(); + } + + // Return the Pigeon2 instance + @SuppressWarnings("unchecked") + public AHRS getGyro() { + return navx; + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = navx.isConnected(); + inputs.yawPosition = Rotation2d.fromDegrees(navx.getYaw()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(navx.getRate()); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 0ada9d66..d1b37cfc 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -56,7 +56,7 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal turnAppliedVolts; private final StatusSignal turnCurrent; - // Gear ratios for SDS MK4i L2, adjust as necessary + // Gear ratios private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); private final double TURN_GEAR_RATIO = 150.0 / 7.0; diff --git a/src/main/java/frc/robot/util/YagslConstants.java b/src/main/java/frc/robot/util/YagslConstants.java index 3bc728e9..02904e90 100644 --- a/src/main/java/frc/robot/util/YagslConstants.java +++ b/src/main/java/frc/robot/util/YagslConstants.java @@ -167,9 +167,9 @@ public class YagslConstants { public static final String kBackRightSteerCanbus = brModule.angle.canbus; public static final String kBackRightEncoderCanbus = brModule.encoder.canbus; public static final double kBackRightEncoderOffset = brModule.absoluteEncoderOffset; - public static final boolean KBackRightDriveInvert = brModule.inverted.drive; - public static final boolean KBackRightSteerInvert = brModule.inverted.angle; - public static final boolean KBackRightEncoderInvert = brModule.absoluteEncoderInverted; + public static final boolean kBackRightDriveInvert = brModule.inverted.drive; + public static final boolean kBackRightSteerInvert = brModule.inverted.angle; + public static final boolean kBackRightEncoderInvert = brModule.absoluteEncoderInverted; public static final double kBackRightXPosInches = brModule.location.left; public static final double kBackRightYPosInches = brModule.location.front; From a3cbd3e106f9e7ebfdeb61ccfabc8c3ebef628a0 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 22 Nov 2024 16:59:10 -0700 Subject: [PATCH 49/57] Remove constants from drive IO files Redirect all constant values to ``DriveConstants`` module. modified: src/main/deploy/swerve/modules/physicalproperties.json modified: src/main/java/frc/robot/subsystems/drive/Drive.java modified: src/main/java/frc/robot/subsystems/drive/DriveConstants.java modified: src/main/java/frc/robot/subsystems/drive/GyroIONavX.java modified: src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java modified: src/main/java/frc/robot/subsystems/drive/Module.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java new file: src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java new file: src/main/java/frc/robot/subsystems/drive/README modified: src/main/java/frc/robot/util/YagslConstants.java --- .../swerve/modules/physicalproperties.json | 4 +- .../frc/robot/subsystems/drive/Drive.java | 16 +- .../subsystems/drive/DriveConstants.java | 62 ++++--- .../robot/subsystems/drive/GyroIONavX.java | 6 - .../robot/subsystems/drive/GyroIOPigeon2.java | 18 +- .../frc/robot/subsystems/drive/Module.java | 10 +- .../subsystems/drive/ModuleIOBlended.java | 70 +++++--- .../drive/ModuleIOSparkCANcoder.java | 170 ++++++++++++++++++ .../subsystems/drive/ModuleIOSparkMax.java | 66 ++++--- .../subsystems/drive/ModuleIOTalonFX.java | 72 +++++--- .../java/frc/robot/subsystems/drive/README | 19 ++ .../java/frc/robot/util/YagslConstants.java | 11 +- 12 files changed, 382 insertions(+), 142 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java create mode 100644 src/main/java/frc/robot/subsystems/drive/README diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index d43b4d62..7708865f 100644 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -2,8 +2,8 @@ "optimalVoltage": 12, "wheelGripCoefficientOfFriction": 1.19, "currentLimit": { - "drive": 40, - "angle": 20 + "drive": 120, + "angle": 40 }, "conversionFactors": { "angle": { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 5105072a..80bad4aa 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -39,6 +39,7 @@ import frc.robot.Constants; import frc.robot.Constants.DrivebaseConstants; import frc.robot.util.LocalADStarAK; +import frc.robot.util.YagslConstants; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -65,13 +66,22 @@ public class Drive extends SubsystemBase { public Drive(Constants.SwerveType swerveType) { switch (swerveType) { case PHOENIX6: + // This one is easy because it's all CTRE gyroIO = new GyroIOPigeon2(DriveConstants.kPigeonId, DriveConstants.kCANbusName); modules[0] = new Module(new ModuleIOTalonFX(0), 0); - modules[0] = new Module(new ModuleIOTalonFX(0), 0); - modules[0] = new Module(new ModuleIOTalonFX(0), 0); - modules[0] = new Module(new ModuleIOTalonFX(0), 0); + modules[1] = new Module(new ModuleIOTalonFX(1), 1); + modules[2] = new Module(new ModuleIOTalonFX(2), 2); + modules[3] = new Module(new ModuleIOTalonFX(3), 3); + break; case YAGSL: + // This one requires a bit more logic... + if (YagslConstants.swerveDriveJson.imu.type == "pigeon2") { + gyroIO = new GyroIOPigeon2(DriveConstants.kPigeonId, DriveConstants.kCANbusName); + } else if (YagslConstants.swerveDriveJson.imu.type == "navx" + || YagslConstants.swerveDriveJson.imu.type == "navx_spi") { + gyroIO = new GyroIONavX(); + } default: } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 1ae0f145..32162e28 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -13,6 +13,7 @@ package frc.robot.subsystems.drive; +import edu.wpi.first.math.util.Units; import frc.robot.Constants; import frc.robot.generated.TunerConstants; import frc.robot.util.YagslConstants; @@ -36,13 +37,16 @@ public class DriveConstants { public static final double kDriveInertia; public static final double kSteerFrictionVoltage; public static final double kDriveFrictionVoltage; + public static final double kSteerCurrentLimit; + public static final double kDriveCurrentLimit; + public static final double kOptimalVoltage; public static final int kFrontLeftDriveMotorId; public static final int kFrontLeftSteerMotorId; public static final int kFrontLeftEncoderId; public static final String kFrontLeftDriveCanbus; public static final String kFrontLeftSteerCanbus; public static final String kFrontLeftEncoderCanbus; - public static final double kFrontLeftEncoderOffset; + public static final double kFrontLeftEncoderOffset; // In Radians public static final boolean kFrontLeftDriveInvert; public static final boolean kFrontLeftSteerInvert; public static final boolean kFrontLeftEncoderInvert; @@ -54,10 +58,10 @@ public class DriveConstants { public static final String kFrontRightDriveCanbus; public static final String kFrontRightSteerCanbus; public static final String kFrontRightEncoderCanbus; - public static final double kFrontRightEncoderOffset; - public static final boolean KFrontRightDriveInvert; - public static final boolean KFrontRightSteerInvert; - public static final boolean KFrontRightEncoderInvert; + public static final double kFrontRightEncoderOffset; // In Radians + public static final boolean kFrontRightDriveInvert; + public static final boolean kFrontRightSteerInvert; + public static final boolean kFrontRightEncoderInvert; public static final double kFrontRightXPosInches; public static final double kFrontRightYPosInches; public static final int kBackLeftDriveMotorId; @@ -66,7 +70,7 @@ public class DriveConstants { public static final String kBackLeftDriveCanbus; public static final String kBackLeftSteerCanbus; public static final String kBackLeftEncoderCanbus; - public static final double kBackLeftEncoderOffset; + public static final double kBackLeftEncoderOffset; // In Radians public static final boolean kBackLeftDriveInvert; public static final boolean kBackLeftSteerInvert; public static final boolean kBackLeftEncoderInvert; @@ -78,7 +82,7 @@ public class DriveConstants { public static final String kBackRightDriveCanbus; public static final String kBackRightSteerCanbus; public static final String kBackRightEncoderCanbus; - public static final double kBackRightEncoderOffset; + public static final double kBackRightEncoderOffset; // In Radians public static final boolean kBackRightDriveInvert; public static final boolean kBackRightSteerInvert; public static final boolean kBackRightEncoderInvert; @@ -109,13 +113,16 @@ public class DriveConstants { kDriveInertia = TunerConstants.kDriveInertia; kSteerFrictionVoltage = TunerConstants.kSteerFrictionVoltage; kDriveFrictionVoltage = TunerConstants.kDriveFrictionVoltage; + kSteerCurrentLimit = 40.0; // Example from CTRE documentation + kDriveCurrentLimit = 120.0; // Example from CTRE documentation + kOptimalVoltage = 12.0; // Assumed Ideal kFrontLeftDriveMotorId = TunerConstants.kFrontLeftDriveMotorId; kFrontLeftSteerMotorId = TunerConstants.kFrontLeftSteerMotorId; kFrontLeftEncoderId = TunerConstants.kFrontLeftEncoderId; kFrontLeftDriveCanbus = TunerConstants.kCANbusName; kFrontLeftSteerCanbus = TunerConstants.kCANbusName; kFrontLeftEncoderCanbus = TunerConstants.kCANbusName; - kFrontLeftEncoderOffset = TunerConstants.kFrontLeftEncoderOffset; + kFrontLeftEncoderOffset = Units.rotationsToRadians(TunerConstants.kFrontLeftEncoderOffset); kFrontLeftDriveInvert = TunerConstants.kInvertLeftSide; kFrontLeftSteerInvert = TunerConstants.kFrontLeftSteerInvert; kFrontLeftEncoderInvert = false; @@ -127,10 +134,11 @@ public class DriveConstants { kFrontRightDriveCanbus = TunerConstants.kCANbusName; kFrontRightSteerCanbus = TunerConstants.kCANbusName; kFrontRightEncoderCanbus = TunerConstants.kCANbusName; - kFrontRightEncoderOffset = TunerConstants.kFrontRightEncoderOffset; - KFrontRightDriveInvert = TunerConstants.kInvertRightSide; - KFrontRightSteerInvert = TunerConstants.kFrontRightSteerInvert; - KFrontRightEncoderInvert = false; + kFrontRightEncoderOffset = + Units.rotationsToRadians(TunerConstants.kFrontRightEncoderOffset); + kFrontRightDriveInvert = TunerConstants.kInvertRightSide; + kFrontRightSteerInvert = TunerConstants.kFrontRightSteerInvert; + kFrontRightEncoderInvert = false; kFrontRightXPosInches = TunerConstants.kFrontRightXPosInches; kFrontRightYPosInches = TunerConstants.kFrontRightYPosInches; kBackLeftDriveMotorId = TunerConstants.kBackLeftDriveMotorId; @@ -139,7 +147,7 @@ public class DriveConstants { kBackLeftDriveCanbus = TunerConstants.kCANbusName; kBackLeftSteerCanbus = TunerConstants.kCANbusName; kBackLeftEncoderCanbus = TunerConstants.kCANbusName; - kBackLeftEncoderOffset = TunerConstants.kBackLeftEncoderOffset; + kBackLeftEncoderOffset = Units.rotationsToRadians(TunerConstants.kBackLeftEncoderOffset); kBackLeftDriveInvert = TunerConstants.kInvertLeftSide; kBackLeftSteerInvert = TunerConstants.kBackLeftSteerInvert; kBackLeftEncoderInvert = false; @@ -151,7 +159,7 @@ public class DriveConstants { kBackRightDriveCanbus = TunerConstants.kCANbusName; kBackRightSteerCanbus = TunerConstants.kCANbusName; kBackRightEncoderCanbus = TunerConstants.kCANbusName; - kBackRightEncoderOffset = TunerConstants.kBackRightEncoderOffset; + kBackRightEncoderOffset = Units.rotationsToRadians(TunerConstants.kBackRightEncoderOffset); kBackRightDriveInvert = TunerConstants.kInvertRightSide; kBackRightSteerInvert = TunerConstants.kBackRightSteerInvert; kBackRightEncoderInvert = false; @@ -180,13 +188,16 @@ public class DriveConstants { kDriveInertia = YagslConstants.kDriveInertia; kSteerFrictionVoltage = YagslConstants.kSteerFrictionVoltage; kDriveFrictionVoltage = YagslConstants.kDriveFrictionVoltage; + kSteerCurrentLimit = YagslConstants.kSteerCurrentLimit; + kDriveCurrentLimit = YagslConstants.kDriveCurrentLimit; + kOptimalVoltage = YagslConstants.kOptimalVoltage; kFrontLeftDriveMotorId = YagslConstants.kFrontLeftDriveMotorId; kFrontLeftSteerMotorId = YagslConstants.kFrontLeftSteerMotorId; kFrontLeftEncoderId = YagslConstants.kFrontLeftEncoderId; kFrontLeftDriveCanbus = YagslConstants.kFrontLeftDriveCanbus; kFrontLeftSteerCanbus = YagslConstants.kFrontLeftSteerCanbus; kFrontLeftEncoderCanbus = YagslConstants.kFrontLeftEncoderCanbus; - kFrontLeftEncoderOffset = YagslConstants.kFrontLeftEncoderOffset; + kFrontLeftEncoderOffset = Units.degreesToRadians(YagslConstants.kFrontLeftEncoderOffset); kFrontLeftDriveInvert = YagslConstants.kFrontLeftDriveInvert; kFrontLeftSteerInvert = YagslConstants.kFrontLeftSteerInvert; kFrontLeftEncoderInvert = YagslConstants.kFrontLeftEncoderInvert; @@ -198,10 +209,10 @@ public class DriveConstants { kFrontRightDriveCanbus = YagslConstants.kFrontRightDriveCanbus; kFrontRightSteerCanbus = YagslConstants.kFrontRightSteerCanbus; kFrontRightEncoderCanbus = YagslConstants.kFrontRightEncoderCanbus; - kFrontRightEncoderOffset = YagslConstants.kFrontRightEncoderOffset; - KFrontRightDriveInvert = YagslConstants.KFrontRightDriveInvert; - KFrontRightSteerInvert = YagslConstants.KFrontRightSteerInvert; - KFrontRightEncoderInvert = YagslConstants.KFrontRightEncoderInvert; + kFrontRightEncoderOffset = Units.degreesToRadians(YagslConstants.kFrontRightEncoderOffset); + kFrontRightDriveInvert = YagslConstants.kFrontRightDriveInvert; + kFrontRightSteerInvert = YagslConstants.kFrontRightSteerInvert; + kFrontRightEncoderInvert = YagslConstants.kFrontRightEncoderInvert; kFrontRightXPosInches = YagslConstants.kFrontRightXPosInches; kFrontRightYPosInches = YagslConstants.kFrontRightYPosInches; kBackLeftDriveMotorId = YagslConstants.kBackLeftDriveMotorId; @@ -210,7 +221,7 @@ public class DriveConstants { kBackLeftDriveCanbus = YagslConstants.kBackLeftDriveCanbus; kBackLeftSteerCanbus = YagslConstants.kBackLeftSteerCanbus; kBackLeftEncoderCanbus = YagslConstants.kBackLeftEncoderCanbus; - kBackLeftEncoderOffset = YagslConstants.kBackLeftEncoderOffset; + kBackLeftEncoderOffset = Units.degreesToRadians(YagslConstants.kBackLeftEncoderOffset); kBackLeftDriveInvert = YagslConstants.kBackLeftDriveInvert; kBackLeftSteerInvert = YagslConstants.kBackLeftSteerInvert; kBackLeftEncoderInvert = YagslConstants.kBackLeftEncoderInvert; @@ -222,7 +233,7 @@ public class DriveConstants { kBackRightDriveCanbus = YagslConstants.kBackRightDriveCanbus; kBackRightSteerCanbus = YagslConstants.kBackRightSteerCanbus; kBackRightEncoderCanbus = YagslConstants.kBackRightEncoderCanbus; - kBackRightEncoderOffset = YagslConstants.kBackRightEncoderOffset; + kBackRightEncoderOffset = Units.degreesToRadians(YagslConstants.kBackRightEncoderOffset); kBackRightDriveInvert = YagslConstants.kBackRightDriveInvert; kBackRightSteerInvert = YagslConstants.kBackRightSteerInvert; kBackRightEncoderInvert = YagslConstants.kBackRightEncoderInvert; @@ -251,6 +262,9 @@ public class DriveConstants { kDriveInertia = 0.0; kSteerFrictionVoltage = 0.0; kDriveFrictionVoltage = 0.0; + kSteerCurrentLimit = 0.0; + kDriveCurrentLimit = 0.0; + kOptimalVoltage = 0.0; kFrontLeftDriveMotorId = 0; kFrontLeftSteerMotorId = 0; kFrontLeftEncoderId = 0; @@ -270,9 +284,9 @@ public class DriveConstants { kFrontRightSteerCanbus = ""; kFrontRightEncoderCanbus = ""; kFrontRightEncoderOffset = 0.0; - KFrontRightDriveInvert = false; - KFrontRightSteerInvert = false; - KFrontRightEncoderInvert = false; + kFrontRightDriveInvert = false; + kFrontRightSteerInvert = false; + kFrontRightEncoderInvert = false; kFrontRightXPosInches = 0.0; kFrontRightYPosInches = 0.0; kBackLeftDriveMotorId = 0; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index ca20381d..0b8c8364 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -30,12 +30,6 @@ public GyroIONavX() { navx.reset(); } - // Constructor, taking deviceID and canBus (which are ignored, since it is connected via SPI) - public GyroIONavX(int deviceID, String canBus) { - navx = new AHRS(SPI.Port.kMXP, (byte) 100.0); - navx.reset(); - } - // Return the Pigeon2 instance @SuppressWarnings("unchecked") public AHRS getGyro() { diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 1e67f958..1caa44d1 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -15,6 +15,8 @@ package frc.robot.subsystems.drive; +import static frc.robot.subsystems.drive.DriveConstants.*; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; @@ -29,21 +31,9 @@ public class GyroIOPigeon2 implements GyroIO { private final StatusSignal yaw; private final StatusSignal yawVelocity; - // Constructor, taking default values + // Constructor public GyroIOPigeon2() { - pigeon = new Pigeon2(20); - pigeon.getConfigurator().apply(new Pigeon2Configuration()); - pigeon.getConfigurator().setYaw(0.0); - yaw = pigeon.getYaw(); - yawVelocity = pigeon.getAngularVelocityZWorld(); - yaw.setUpdateFrequency(100.0); - yawVelocity.setUpdateFrequency(100.0); - pigeon.optimizeBusUtilization(); - } - - // Constructor, taking deviceID and canBus - public GyroIOPigeon2(int deviceID, String canBus) { - pigeon = new Pigeon2(deviceID, canBus); + pigeon = new Pigeon2(kBackLeftEncoderId, kBackLeftEncoderCanbus); pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); yaw = pigeon.getYaw(); diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index ff296dce..9334b159 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -15,6 +15,8 @@ package frc.robot.subsystems.drive; +import static frc.robot.subsystems.drive.DriveConstants.*; + import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; @@ -25,7 +27,7 @@ import org.littletonrobotics.junction.Logger; public class Module { - private static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); + private static final double WHEEL_RADIUS = Units.inchesToMeters(kWheelRadiusInches); private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); @@ -47,9 +49,9 @@ public Module(ModuleIO io, int index) { switch (Constants.getMode()) { case REAL: case REPLAY: - driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - driveFeedback = new PIDController(0.05, 0.0, 0.0); - turnFeedback = new PIDController(7.0, 0.0, 0.0); + driveFeedforward = new SimpleMotorFeedforward(kDriveFrictionVoltage, kDriveF); + driveFeedback = new PIDController(kDriveP, kDriveI, kDriveD); + turnFeedback = new PIDController(kSteerP, kSteerI, kSteerD); break; case SIM: driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index 447e3909..9dfbfb05 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -15,6 +15,8 @@ package frc.robot.subsystems.drive; +import static frc.robot.subsystems.drive.DriveConstants.*; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; @@ -57,51 +59,61 @@ public class ModuleIOBlended implements ModuleIO { private final StatusSignal turnAbsolutePosition; private final RelativeEncoder turnRelativeEncoder; - // Gear ratios for SDS MK4i L2, adjust as necessary - private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private final double TURN_GEAR_RATIO = 150.0 / 7.0; - - private final boolean isTurnMotorInverted = true; + private final boolean isTurnMotorInverted; private final Rotation2d absoluteEncoderOffset; /* - * NutBlend Module I/O, using Falcon drive and NEO steer motors + * Blended Module I/O, using Falcon drive and NEO steer motors * Based on the ModuleIOTalonFX module, with the SparkMax components * added in appropriately. */ public ModuleIOBlended(int index) { switch (index) { case 0: - driveTalon = new TalonFX(0); - turnSparkMax = new CANSparkMax(1, MotorType.kBrushless); - cancoder = new CANcoder(2); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + // Front Left + driveTalon = new TalonFX(kFrontLeftDriveMotorId, kFrontLeftDriveCanbus); + turnSparkMax = new CANSparkMax(kFrontLeftSteerMotorId, MotorType.kBrushless); + cancoder = new CANcoder(kFrontLeftEncoderId, kFrontLeftEncoderCanbus); + absoluteEncoderOffset = new Rotation2d(kFrontLeftEncoderOffset); + isTurnMotorInverted = kFrontLeftSteerInvert; break; + case 1: - driveTalon = new TalonFX(3); - turnSparkMax = new CANSparkMax(4, MotorType.kBrushless); - cancoder = new CANcoder(5); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + // Front Right + driveTalon = new TalonFX(kFrontRightDriveMotorId, kFrontRightDriveCanbus); + turnSparkMax = new CANSparkMax(kFrontRightSteerMotorId, MotorType.kBrushless); + cancoder = new CANcoder(kFrontRightEncoderId, kFrontRightEncoderCanbus); + absoluteEncoderOffset = new Rotation2d(kFrontRightEncoderOffset); + isTurnMotorInverted = kFrontRightSteerInvert; break; + case 2: - driveTalon = new TalonFX(6); - turnSparkMax = new CANSparkMax(7, MotorType.kBrushless); - cancoder = new CANcoder(8); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + // Back Left + driveTalon = new TalonFX(kBackLeftDriveMotorId, kBackLeftDriveCanbus); + turnSparkMax = new CANSparkMax(kBackLeftSteerMotorId, MotorType.kBrushless); + cancoder = new CANcoder(kBackLeftEncoderId, kBackLeftEncoderCanbus); + absoluteEncoderOffset = new Rotation2d(kBackLeftEncoderOffset); + isTurnMotorInverted = kBackLeftSteerInvert; break; + case 3: - driveTalon = new TalonFX(9); - turnSparkMax = new CANSparkMax(10, MotorType.kBrushless); - cancoder = new CANcoder(11); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + // Back Right + driveTalon = new TalonFX(kBackRightDriveMotorId, kBackRightDriveCanbus); + turnSparkMax = new CANSparkMax(kBackRightSteerMotorId, MotorType.kBrushless); + cancoder = new CANcoder(kBackRightEncoderId, kBackRightEncoderCanbus); + absoluteEncoderOffset = new Rotation2d(kBackRightEncoderOffset); + isTurnMotorInverted = kBackRightSteerInvert; break; + default: throw new RuntimeException("Invalid module index"); } // Drive Configuration var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + driveConfig.CurrentLimits.StatorCurrentLimit = kDriveCurrentLimit; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.CurrentLimits.SupplyCurrentLimit = kDriveCurrentLimit * 0.6; driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; driveTalon.getConfigurator().apply(driveConfig); setDriveBrakeMode(true); @@ -111,8 +123,8 @@ public ModuleIOBlended(int index) { turnSparkMax.setCANTimeout(250); turnRelativeEncoder = turnSparkMax.getEncoder(); turnSparkMax.setInverted(isTurnMotorInverted); - turnSparkMax.setSmartCurrentLimit(30); - turnSparkMax.enableVoltageCompensation(12.0); + turnSparkMax.setSmartCurrentLimit((int) kSteerCurrentLimit); + turnSparkMax.enableVoltageCompensation(kOptimalVoltage); turnRelativeEncoder.setPosition(0.0); turnRelativeEncoder.setMeasurementPeriod(10); turnRelativeEncoder.setAverageDepth(2); @@ -141,9 +153,9 @@ public void updateInputs(ModuleIOInputs inputs) { drivePosition, driveVelocity, driveAppliedVolts, driveCurrent, turnAbsolutePosition); inputs.drivePositionRad = - Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; + Units.rotationsToRadians(drivePosition.getValueAsDouble()) / kDriveGearRatio; inputs.driveVelocityRadPerSec = - Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; + Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / kDriveGearRatio; inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; @@ -151,10 +163,10 @@ public void updateInputs(ModuleIOInputs inputs) { Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()) .minus(absoluteEncoderOffset); inputs.turnPosition = - Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO); + Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / kSteerGearRatio); inputs.turnVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) - / TURN_GEAR_RATIO; + / kSteerGearRatio; inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java new file mode 100644 index 00000000..557c7c88 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java @@ -0,0 +1,170 @@ +// Copyright (c) 2024 Az-FIRST +// http://github.com/AZ-First +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// 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 frc.robot.subsystems.drive; + +import static frc.robot.subsystems.drive.DriveConstants.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; + +/** + * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO + * or NEO 550), and CTRE's CANcoder. + * + *

To calibrate the absolute encoder offsets, point the modules straight (such that forward + * motion on the drive motor will propel the robot forward) and copy the reported values from the + * absolute encoders using AdvantageScope. These values are logged under + * "/Drive/ModuleX/TurnAbsolutePositionRad" + */ +public class ModuleIOSparkCANcoder implements ModuleIO { + private final CANSparkMax driveSparkMax; + private final CANSparkMax turnSparkMax; + + private final RelativeEncoder driveEncoder; + private final RelativeEncoder turnRelativeEncoder; + private final CANcoder cancoder; + private final StatusSignal turnAbsolutePosition; + + private final boolean isTurnMotorInverted; + private final Rotation2d absoluteEncoderOffset; + + public ModuleIOSparkCANcoder(int index) { + switch (index) { + case 0: + // Front Left + driveSparkMax = new CANSparkMax(kFrontLeftDriveMotorId, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(kFrontLeftSteerMotorId, MotorType.kBrushless); + cancoder = new CANcoder(kFrontLeftEncoderId, kFrontLeftEncoderCanbus); + absoluteEncoderOffset = new Rotation2d(kFrontLeftEncoderOffset); + isTurnMotorInverted = kFrontLeftSteerInvert; + break; + + case 1: + // Front Right + driveSparkMax = new CANSparkMax(kFrontRightDriveMotorId, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(kFrontRightSteerMotorId, MotorType.kBrushless); + cancoder = new CANcoder(kFrontRightEncoderId, kFrontRightEncoderCanbus); + absoluteEncoderOffset = new Rotation2d(kFrontRightEncoderOffset); + isTurnMotorInverted = kFrontRightSteerInvert; + break; + + case 2: + // Back Left + driveSparkMax = new CANSparkMax(kBackLeftDriveMotorId, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(kBackLeftSteerMotorId, MotorType.kBrushless); + cancoder = new CANcoder(kBackLeftEncoderId, kBackLeftEncoderCanbus); + absoluteEncoderOffset = new Rotation2d(kBackLeftEncoderOffset); + isTurnMotorInverted = kBackLeftSteerInvert; + break; + + case 3: + // Back Right + driveSparkMax = new CANSparkMax(kBackRightDriveMotorId, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(kBackRightSteerMotorId, MotorType.kBrushless); + cancoder = new CANcoder(kBackRightEncoderId, kBackRightEncoderCanbus); + absoluteEncoderOffset = new Rotation2d(kBackRightEncoderOffset); + isTurnMotorInverted = kBackRightSteerInvert; + break; + + default: + throw new RuntimeException("Invalid module index"); + } + + driveSparkMax.restoreFactoryDefaults(); + turnSparkMax.restoreFactoryDefaults(); + + driveSparkMax.setCANTimeout(250); + turnSparkMax.setCANTimeout(250); + + driveEncoder = driveSparkMax.getEncoder(); + turnRelativeEncoder = turnSparkMax.getEncoder(); + + turnSparkMax.setInverted(isTurnMotorInverted); + driveSparkMax.setSmartCurrentLimit((int) kDriveCurrentLimit); + turnSparkMax.setSmartCurrentLimit((int) kSteerCurrentLimit); + driveSparkMax.enableVoltageCompensation(kOptimalVoltage); + turnSparkMax.enableVoltageCompensation(12.0); + + cancoder.getConfigurator().apply(new CANcoderConfiguration()); + + driveEncoder.setPosition(0.0); + driveEncoder.setMeasurementPeriod(10); + driveEncoder.setAverageDepth(2); + + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod(10); + turnRelativeEncoder.setAverageDepth(2); + + turnAbsolutePosition = cancoder.getAbsolutePosition(); + BaseStatusSignal.setUpdateFrequencyForAll(50.0, turnAbsolutePosition); + driveSparkMax.setCANTimeout(0); + turnSparkMax.setCANTimeout(0); + + driveSparkMax.burnFlash(); + turnSparkMax.burnFlash(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll(turnAbsolutePosition); + + inputs.drivePositionRad = + Units.rotationsToRadians(driveEncoder.getPosition()) / kDriveGearRatio; + inputs.driveVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / kDriveGearRatio; + inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); + inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()}; + + inputs.turnAbsolutePosition = + Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()) + .minus(absoluteEncoderOffset); + inputs.turnPosition = + Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / kSteerGearRatio); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) + / kSteerGearRatio; + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; + } + + @Override + public void setDriveVoltage(double volts) { + driveSparkMax.setVoltage(volts); + } + + @Override + public void setTurnVoltage(double volts) { + turnSparkMax.setVoltage(volts); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index 695755f5..a64643b7 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -15,6 +15,8 @@ package frc.robot.subsystems.drive; +import static frc.robot.subsystems.drive.DriveConstants.*; + import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; @@ -37,10 +39,6 @@ * "/Drive/ModuleX/TurnAbsolutePositionRad" */ public class ModuleIOSparkMax implements ModuleIO { - // Gear ratios for SDS MK4i L2, adjust as necessary - private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private static final double TURN_GEAR_RATIO = 150.0 / 7.0; - private final CANSparkMax driveSparkMax; private final CANSparkMax turnSparkMax; @@ -48,35 +46,47 @@ public class ModuleIOSparkMax implements ModuleIO { private final RelativeEncoder turnRelativeEncoder; private final AnalogInput turnAbsoluteEncoder; - private final boolean isTurnMotorInverted = true; + private final boolean isTurnMotorInverted; private final Rotation2d absoluteEncoderOffset; public ModuleIOSparkMax(int index) { switch (index) { case 0: - driveSparkMax = new CANSparkMax(1, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(2, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(0); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + // Front Left + driveSparkMax = new CANSparkMax(kFrontLeftDriveMotorId, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(kFrontLeftSteerMotorId, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(kFrontLeftEncoderId); + absoluteEncoderOffset = new Rotation2d(kFrontLeftEncoderOffset); + isTurnMotorInverted = kFrontLeftSteerInvert; break; + case 1: - driveSparkMax = new CANSparkMax(3, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(4, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(1); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + // Front Right + driveSparkMax = new CANSparkMax(kFrontRightDriveMotorId, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(kFrontRightSteerMotorId, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(kFrontRightEncoderId); + absoluteEncoderOffset = new Rotation2d(kFrontRightEncoderOffset); + isTurnMotorInverted = kFrontRightSteerInvert; break; + case 2: - driveSparkMax = new CANSparkMax(5, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(6, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(2); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + // Back Left + driveSparkMax = new CANSparkMax(kBackLeftDriveMotorId, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(kBackLeftSteerMotorId, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(kBackLeftEncoderId); + absoluteEncoderOffset = new Rotation2d(kBackLeftEncoderOffset); + isTurnMotorInverted = kBackLeftSteerInvert; break; + case 3: - driveSparkMax = new CANSparkMax(7, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(8, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(3); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + // Back Right + driveSparkMax = new CANSparkMax(kBackRightDriveMotorId, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(kBackRightSteerMotorId, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(kBackRightEncoderId); + absoluteEncoderOffset = new Rotation2d(kBackRightEncoderOffset); + isTurnMotorInverted = kBackRightSteerInvert; break; + default: throw new RuntimeException("Invalid module index"); } @@ -91,9 +101,9 @@ public ModuleIOSparkMax(int index) { turnRelativeEncoder = turnSparkMax.getEncoder(); turnSparkMax.setInverted(isTurnMotorInverted); - driveSparkMax.setSmartCurrentLimit(40); - turnSparkMax.setSmartCurrentLimit(30); - driveSparkMax.enableVoltageCompensation(12.0); + driveSparkMax.setSmartCurrentLimit((int) kDriveCurrentLimit); + turnSparkMax.setSmartCurrentLimit((int) kSteerCurrentLimit); + driveSparkMax.enableVoltageCompensation(kOptimalVoltage); turnSparkMax.enableVoltageCompensation(12.0); driveEncoder.setPosition(0.0); @@ -114,9 +124,9 @@ public ModuleIOSparkMax(int index) { @Override public void updateInputs(ModuleIOInputs inputs) { inputs.drivePositionRad = - Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO; + Units.rotationsToRadians(driveEncoder.getPosition()) / kDriveGearRatio; inputs.driveVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO; + Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / kDriveGearRatio; inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()}; @@ -125,10 +135,10 @@ public void updateInputs(ModuleIOInputs inputs) { turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V() * 2.0 * Math.PI) .minus(absoluteEncoderOffset); inputs.turnPosition = - Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO); + Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / kSteerGearRatio); inputs.turnVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) - / TURN_GEAR_RATIO; + / kSteerGearRatio; inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index d1b37cfc..b7008711 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -15,6 +15,8 @@ package frc.robot.subsystems.drive; +import static frc.robot.subsystems.drive.DriveConstants.*; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; @@ -56,52 +58,64 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal turnAppliedVolts; private final StatusSignal turnCurrent; - // Gear ratios - private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private final double TURN_GEAR_RATIO = 150.0 / 7.0; - - private final boolean isTurnMotorInverted = true; + private final boolean isTurnMotorInverted; private final Rotation2d absoluteEncoderOffset; public ModuleIOTalonFX(int index) { switch (index) { case 0: - driveTalon = new TalonFX(0); - turnTalon = new TalonFX(1); - cancoder = new CANcoder(2); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + // Front Left + driveTalon = new TalonFX(kFrontLeftDriveMotorId, kFrontLeftDriveCanbus); + turnTalon = new TalonFX(kFrontLeftSteerMotorId, kFrontLeftSteerCanbus); + cancoder = new CANcoder(kFrontLeftEncoderId, kFrontLeftEncoderCanbus); + absoluteEncoderOffset = new Rotation2d(kFrontLeftEncoderOffset); + isTurnMotorInverted = kFrontLeftSteerInvert; break; + case 1: - driveTalon = new TalonFX(3); - turnTalon = new TalonFX(4); - cancoder = new CANcoder(5); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + // Front Right + driveTalon = new TalonFX(kFrontRightDriveMotorId, kFrontRightDriveCanbus); + turnTalon = new TalonFX(kFrontRightSteerMotorId, kFrontRightSteerCanbus); + cancoder = new CANcoder(kFrontRightEncoderId, kFrontRightEncoderCanbus); + absoluteEncoderOffset = new Rotation2d(kFrontRightEncoderOffset); + isTurnMotorInverted = DriveConstants.kFrontRightSteerInvert; break; + case 2: - driveTalon = new TalonFX(6); - turnTalon = new TalonFX(7); - cancoder = new CANcoder(8); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + // Back Left + driveTalon = new TalonFX(kBackLeftDriveMotorId, kBackLeftDriveCanbus); + turnTalon = new TalonFX(kBackLeftSteerMotorId, kBackLeftSteerCanbus); + cancoder = new CANcoder(kBackLeftEncoderId, kBackLeftEncoderCanbus); + absoluteEncoderOffset = new Rotation2d(kBackLeftEncoderOffset); + isTurnMotorInverted = DriveConstants.kBackLeftSteerInvert; break; + case 3: - driveTalon = new TalonFX(9); - turnTalon = new TalonFX(10); - cancoder = new CANcoder(11); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + // Back Right + driveTalon = new TalonFX(kBackRightDriveMotorId, kBackRightDriveCanbus); + turnTalon = new TalonFX(kBackRightSteerMotorId, kBackRightSteerCanbus); + cancoder = new CANcoder(kBackRightEncoderId, kBackRightEncoderCanbus); + absoluteEncoderOffset = new Rotation2d(kBackRightEncoderOffset); + isTurnMotorInverted = DriveConstants.kBackRightSteerInvert; break; + default: throw new RuntimeException("Invalid module index"); } + // Example values from + // https://v6.docs.ctr-electronics.com/en/stable/docs/hardware-reference/talonfx/improving-performance-with-current-limits.html var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; - driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveConfig.CurrentLimits.StatorCurrentLimit = kDriveCurrentLimit; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.CurrentLimits.SupplyCurrentLimit = kDriveCurrentLimit * 0.6; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; driveTalon.getConfigurator().apply(driveConfig); setDriveBrakeMode(true); var turnConfig = new TalonFXConfiguration(); - turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; - turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + turnConfig.CurrentLimits.StatorCurrentLimit = kSteerCurrentLimit; + turnConfig.CurrentLimits.StatorCurrentLimitEnable = true; turnTalon.getConfigurator().apply(turnConfig); setTurnBrakeMode(true); @@ -147,9 +161,9 @@ public void updateInputs(ModuleIOInputs inputs) { turnCurrent); inputs.drivePositionRad = - Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; + Units.rotationsToRadians(drivePosition.getValueAsDouble()) / kDriveGearRatio; inputs.driveVelocityRadPerSec = - Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; + Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / kDriveGearRatio; inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; @@ -157,9 +171,9 @@ public void updateInputs(ModuleIOInputs inputs) { Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()) .minus(absoluteEncoderOffset); inputs.turnPosition = - Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO); + Rotation2d.fromRotations(turnPosition.getValueAsDouble() / kSteerGearRatio); inputs.turnVelocityRadPerSec = - Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO; + Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / kSteerGearRatio; inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; } diff --git a/src/main/java/frc/robot/subsystems/drive/README b/src/main/java/frc/robot/subsystems/drive/README new file mode 100644 index 00000000..8efb8405 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/README @@ -0,0 +1,19 @@ +The hardware-specific IO files included here are a baseline for what we expect +teams to use. If, however, you are using a different combination of hardware, +you will need to create a new ``ModuleIO*.java`` or ``GyroIO*.java`` file. We +encourage you to submit a PR to the Az-RBSI repositiory with your new hardware +file if you feel other teams may benefit from its inclusion. + +Existing IO files: + + - ``GyroIOPigeon2.java``: CTRE Pigeon2 as the swerve IMU + - ``GyroIONavX.java``: NavX as the swerve IMU, connected via SPI + + - ``ModuleIOTalonFX.java``: 2x TalonFX motors + CANcoder + - ``ModuleIOSparkMax.java``: 2x Rev NEO motors + analog absolute encoder connected to the RIO + - ``ModuleIOSparkCANcoder.java``: 2x Rv NEO motors + CANcoder + - ``ModuleIOBlended.java``: TalonFX drive, Rev NEO steer motors + CANcoder + + +-------------------- +Last Modified: 22 Nov 2024, TPEB diff --git a/src/main/java/frc/robot/util/YagslConstants.java b/src/main/java/frc/robot/util/YagslConstants.java index 02904e90..567f15fa 100644 --- a/src/main/java/frc/robot/util/YagslConstants.java +++ b/src/main/java/frc/robot/util/YagslConstants.java @@ -110,6 +110,11 @@ public class YagslConstants { public static final double kSteerFrictionVoltage = 0.25; public static final double kDriveFrictionVoltage = 0.25; + // Current / Voltage Limits + public static final double kSteerCurrentLimit = physicalPropertiesJson.currentLimit.angle; + public static final double kDriveCurrentLimit = physicalPropertiesJson.currentLimit.drive; + public static final double kOptimalVoltage = physicalPropertiesJson.optimalVoltage; + // The modules themselves static List moduleList = Arrays.asList(swerveDriveJson.modules); @@ -137,9 +142,9 @@ public class YagslConstants { public static final String kFrontRightSteerCanbus = frModule.angle.canbus; public static final String kFrontRightEncoderCanbus = frModule.encoder.canbus; public static final double kFrontRightEncoderOffset = frModule.absoluteEncoderOffset; - public static final boolean KFrontRightDriveInvert = frModule.inverted.drive; - public static final boolean KFrontRightSteerInvert = frModule.inverted.angle; - public static final boolean KFrontRightEncoderInvert = frModule.absoluteEncoderInverted; + public static final boolean kFrontRightDriveInvert = frModule.inverted.drive; + public static final boolean kFrontRightSteerInvert = frModule.inverted.angle; + public static final boolean kFrontRightEncoderInvert = frModule.absoluteEncoderInverted; public static final double kFrontRightXPosInches = frModule.location.left; public static final double kFrontRightYPosInches = frModule.location.front; From d34f392a3e9cc8803bce12613d042effa753375f Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 22 Nov 2024 18:12:23 -0700 Subject: [PATCH 50/57] Drive constructor is complete There was some tricky logic to be able to parse out which module type to instantiate based on what's in the YAGSL config, but this allows for generality derived simply from the JSON files. modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/subsystems/drive/Drive.java modified: src/main/java/frc/robot/subsystems/drive/DriveConstants.java modified: src/main/java/frc/robot/subsystems/drive/README modified: src/main/java/frc/robot/util/YagslConstants.java --- src/main/java/frc/robot/RobotContainer.java | 6 +- .../frc/robot/subsystems/drive/Drive.java | 122 +++++++++++++----- .../subsystems/drive/DriveConstants.java | 52 ++++++++ .../java/frc/robot/subsystems/drive/README | 4 +- .../java/frc/robot/util/YagslConstants.java | 12 ++ 5 files changed, 162 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a48e6e53..507bcec2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -96,7 +96,7 @@ public RobotContainer() { case REAL: // Real robot, instantiate hardware IO implementations // YAGSL drivebase, get config from deploy directory - m_drivebase = new Drive(Constants.getSwerveType()); + m_drivebase = new Drive(); m_flywheel = new Flywheel(new FlywheelIOSim()); // new Flywheel(new FlywheelIOTalonFX()); m_vision = new Vision( @@ -107,7 +107,7 @@ public RobotContainer() { case SIM: // Sim robot, instantiate physics sim IO implementations - m_drivebase = new Drive(Constants.getSwerveType()); + m_drivebase = new Drive(); m_flywheel = new Flywheel(new FlywheelIOSim()); m_vision = new Vision(this::getAprilTagLayoutType); m_accel = new Accelerometer(m_drivebase.getGyro()); @@ -115,7 +115,7 @@ public RobotContainer() { default: // Replayed robot, disable IO implementations - m_drivebase = new Drive(Constants.getSwerveType()); + m_drivebase = new Drive(); m_flywheel = new Flywheel(new FlywheelIO() {}); m_vision = new Vision(this::getAprilTagLayoutType, new VisionIO() {}, new VisionIO() {}); m_accel = new Accelerometer(m_drivebase.getGyro()); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 80bad4aa..3a963157 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -16,6 +16,7 @@ package frc.robot.subsystems.drive; import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.drive.DriveConstants.*; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.pathfinding.Pathfinding; @@ -31,6 +32,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; @@ -62,43 +64,101 @@ public class Drive extends SubsystemBase { private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); - // Constructor that takes ENUM type - public Drive(Constants.SwerveType swerveType) { - switch (swerveType) { + // Constructor + public Drive() { + switch (Constants.getSwerveType()) { case PHOENIX6: - // This one is easy because it's all CTRE - gyroIO = new GyroIOPigeon2(DriveConstants.kPigeonId, DriveConstants.kCANbusName); - modules[0] = new Module(new ModuleIOTalonFX(0), 0); - modules[1] = new Module(new ModuleIOTalonFX(1), 1); - modules[2] = new Module(new ModuleIOTalonFX(2), 2); - modules[3] = new Module(new ModuleIOTalonFX(3), 3); + // This one is easy because it's all CTRE all the time + gyroIO = new GyroIOPigeon2(); + for (int i = 0; i < 4; i++) { + modules[i] = new Module(new ModuleIOTalonFX(i), i); + } break; case YAGSL: - // This one requires a bit more logic... - if (YagslConstants.swerveDriveJson.imu.type == "pigeon2") { - gyroIO = new GyroIOPigeon2(DriveConstants.kPigeonId, DriveConstants.kCANbusName); - } else if (YagslConstants.swerveDriveJson.imu.type == "navx" - || YagslConstants.swerveDriveJson.imu.type == "navx_spi") { - gyroIO = new GyroIONavX(); + // First, choose the Gyro + switch (YagslConstants.swerveDriveJson.imu.type) { + case "pigeon2": + gyroIO = new GyroIOPigeon2(); + break; + case "navx": + case "navx_spi": + gyroIO = new GyroIONavX(); + break; + default: + throw new RuntimeException("Invalid IMU type"); + } + // Then choose the module(s) + // NOTE: This assumes all 4 modules have the same arrangement! + Byte b_drive; // [x,x,-,-,-,-,-,-] + Byte b_steer; // [-,-,x,x,-,-,-,-] + Byte b_encoder; // [-,-,-,-,x,x,-,-] + switch (kFrontLeftDriveType) { + case "falcon": + case "kraken": + case "talonfx": + // CTRE Drive Motor + b_drive = 0b00; + break; + case "sparkmax": + case "sparkflex": + // REV Drive Motor + b_drive = 0b01; + break; + default: + throw new RuntimeException("Invalid drive motor type"); + } + switch (kFrontLeftSteerType) { + case "falcon": + case "kraken": + case "talonfx": + // CTRE Drive Motor + b_steer = 0b00; + break; + case "sparkmax": + case "sparkflex": + // REV Drive Motor + b_steer = 0b01; + break; + default: + throw new RuntimeException("Invalid steer motor type"); + } + switch (kFrontLeftEncoderType) { + case "cancoder": + // CTRE CANcoder + b_encoder = 0b00; + break; + case "analog": + // Analog Encoder + b_encoder = 0b01; + break; + default: + throw new RuntimeException("Invalid swerve encoder type"); + } + Byte modType = (byte) (0b00000000 | b_drive << 6 | b_steer << 4 | b_encoder << 2); + + for (int i = 0; i < 4; i++) { + switch (modType) { + case 0b00000000: // ALL-CTRE + modules[i] = new Module(new ModuleIOTalonFX(i), i); + break; + case 0b00010000: // Blended Talon Drive / NEO Steer + modules[i] = new Module(new ModuleIOBlended(i), i); + break; + case 0b01010000: // NEO motors + CANcoder + modules[i] = new Module(new ModuleIOSparkCANcoder(i), i); + break; + case 0b01010100: // NEO motors + analog encoder + modules[i] = new Module(new ModuleIOSparkMax(i), i); + break; + default: + throw new RuntimeException("Invalid swerve module combination"); + } } default: + throw new RuntimeException("Invalid Swerve Drive Type"); } - } - - // Default constructor - public Drive( - GyroIO gyroIO, - ModuleIO flModuleIO, - ModuleIO frModuleIO, - ModuleIO blModuleIO, - ModuleIO brModuleIO) { - this.gyroIO = gyroIO; - modules[0] = new Module(flModuleIO, 0); - modules[1] = new Module(frModuleIO, 1); - modules[2] = new Module(blModuleIO, 2); - modules[3] = new Module(brModuleIO, 3); // Configure AutoBuilder for PathPlanner AutoBuilder.configureHolonomic( @@ -107,7 +167,9 @@ public Drive( () -> kinematics.toChassisSpeeds(getModuleStates()), this::runVelocity, new HolonomicPathFollowerConfig( - DrivebaseConstants.kMaxLinearSpeed, DRIVE_BASE_RADIUS, new ReplanningConfig()), + DrivebaseConstants.kMaxLinearSpeed, + Units.inchesToMeters(kDriveBaseRadius), + new ReplanningConfig()), () -> DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red, diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 32162e28..5481bada 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -46,6 +46,9 @@ public class DriveConstants { public static final String kFrontLeftDriveCanbus; public static final String kFrontLeftSteerCanbus; public static final String kFrontLeftEncoderCanbus; + public static final String kFrontLeftDriveType; + public static final String kFrontLeftSteerType; + public static final String kFrontLeftEncoderType; public static final double kFrontLeftEncoderOffset; // In Radians public static final boolean kFrontLeftDriveInvert; public static final boolean kFrontLeftSteerInvert; @@ -58,6 +61,9 @@ public class DriveConstants { public static final String kFrontRightDriveCanbus; public static final String kFrontRightSteerCanbus; public static final String kFrontRightEncoderCanbus; + public static final String kFrontRightDriveType; + public static final String kFrontRightSteerType; + public static final String kFrontRightEncoderType; public static final double kFrontRightEncoderOffset; // In Radians public static final boolean kFrontRightDriveInvert; public static final boolean kFrontRightSteerInvert; @@ -70,6 +76,9 @@ public class DriveConstants { public static final String kBackLeftDriveCanbus; public static final String kBackLeftSteerCanbus; public static final String kBackLeftEncoderCanbus; + public static final String kBackLeftDriveType; + public static final String kBackLeftSteerType; + public static final String kBackLeftEncoderType; public static final double kBackLeftEncoderOffset; // In Radians public static final boolean kBackLeftDriveInvert; public static final boolean kBackLeftSteerInvert; @@ -82,6 +91,9 @@ public class DriveConstants { public static final String kBackRightDriveCanbus; public static final String kBackRightSteerCanbus; public static final String kBackRightEncoderCanbus; + public static final String kBackRightDriveType; + public static final String kBackRightSteerType; + public static final String kBackRightEncoderType; public static final double kBackRightEncoderOffset; // In Radians public static final boolean kBackRightDriveInvert; public static final boolean kBackRightSteerInvert; @@ -122,6 +134,9 @@ public class DriveConstants { kFrontLeftDriveCanbus = TunerConstants.kCANbusName; kFrontLeftSteerCanbus = TunerConstants.kCANbusName; kFrontLeftEncoderCanbus = TunerConstants.kCANbusName; + kFrontLeftDriveType = "kraken"; + kFrontLeftSteerType = "kraken"; + kFrontLeftEncoderType = "cancoder"; kFrontLeftEncoderOffset = Units.rotationsToRadians(TunerConstants.kFrontLeftEncoderOffset); kFrontLeftDriveInvert = TunerConstants.kInvertLeftSide; kFrontLeftSteerInvert = TunerConstants.kFrontLeftSteerInvert; @@ -134,6 +149,9 @@ public class DriveConstants { kFrontRightDriveCanbus = TunerConstants.kCANbusName; kFrontRightSteerCanbus = TunerConstants.kCANbusName; kFrontRightEncoderCanbus = TunerConstants.kCANbusName; + kFrontRightDriveType = "kraken"; + kFrontRightSteerType = "kraken"; + kFrontRightEncoderType = "cancoder"; kFrontRightEncoderOffset = Units.rotationsToRadians(TunerConstants.kFrontRightEncoderOffset); kFrontRightDriveInvert = TunerConstants.kInvertRightSide; @@ -147,6 +165,9 @@ public class DriveConstants { kBackLeftDriveCanbus = TunerConstants.kCANbusName; kBackLeftSteerCanbus = TunerConstants.kCANbusName; kBackLeftEncoderCanbus = TunerConstants.kCANbusName; + kBackLeftDriveType = "kraken"; + kBackLeftSteerType = "kraken"; + kBackLeftEncoderType = "cancoder"; kBackLeftEncoderOffset = Units.rotationsToRadians(TunerConstants.kBackLeftEncoderOffset); kBackLeftDriveInvert = TunerConstants.kInvertLeftSide; kBackLeftSteerInvert = TunerConstants.kBackLeftSteerInvert; @@ -159,6 +180,9 @@ public class DriveConstants { kBackRightDriveCanbus = TunerConstants.kCANbusName; kBackRightSteerCanbus = TunerConstants.kCANbusName; kBackRightEncoderCanbus = TunerConstants.kCANbusName; + kBackRightDriveType = "kraken"; + kBackRightSteerType = "kraken"; + kBackRightEncoderType = "cancoder"; kBackRightEncoderOffset = Units.rotationsToRadians(TunerConstants.kBackRightEncoderOffset); kBackRightDriveInvert = TunerConstants.kInvertRightSide; kBackRightSteerInvert = TunerConstants.kBackRightSteerInvert; @@ -197,6 +221,9 @@ public class DriveConstants { kFrontLeftDriveCanbus = YagslConstants.kFrontLeftDriveCanbus; kFrontLeftSteerCanbus = YagslConstants.kFrontLeftSteerCanbus; kFrontLeftEncoderCanbus = YagslConstants.kFrontLeftEncoderCanbus; + kFrontLeftDriveType = YagslConstants.kFrontLeftDriveType.toLowerCase(); + kFrontLeftSteerType = YagslConstants.kFrontLeftSteerType.toLowerCase(); + kFrontLeftEncoderType = YagslConstants.kFrontLeftEncoderType.toLowerCase(); kFrontLeftEncoderOffset = Units.degreesToRadians(YagslConstants.kFrontLeftEncoderOffset); kFrontLeftDriveInvert = YagslConstants.kFrontLeftDriveInvert; kFrontLeftSteerInvert = YagslConstants.kFrontLeftSteerInvert; @@ -209,6 +236,9 @@ public class DriveConstants { kFrontRightDriveCanbus = YagslConstants.kFrontRightDriveCanbus; kFrontRightSteerCanbus = YagslConstants.kFrontRightSteerCanbus; kFrontRightEncoderCanbus = YagslConstants.kFrontRightEncoderCanbus; + kFrontRightDriveType = YagslConstants.kFrontRightDriveType.toLowerCase(); + kFrontRightSteerType = YagslConstants.kFrontRightSteerType.toLowerCase(); + kFrontRightEncoderType = YagslConstants.kFrontRightEncoderType.toLowerCase(); kFrontRightEncoderOffset = Units.degreesToRadians(YagslConstants.kFrontRightEncoderOffset); kFrontRightDriveInvert = YagslConstants.kFrontRightDriveInvert; kFrontRightSteerInvert = YagslConstants.kFrontRightSteerInvert; @@ -221,6 +251,9 @@ public class DriveConstants { kBackLeftDriveCanbus = YagslConstants.kBackLeftDriveCanbus; kBackLeftSteerCanbus = YagslConstants.kBackLeftSteerCanbus; kBackLeftEncoderCanbus = YagslConstants.kBackLeftEncoderCanbus; + kBackLeftDriveType = YagslConstants.kBackLeftDriveType.toLowerCase(); + kBackLeftSteerType = YagslConstants.kBackLeftSteerType.toLowerCase(); + kBackLeftEncoderType = YagslConstants.kBackLeftEncoderType.toLowerCase(); kBackLeftEncoderOffset = Units.degreesToRadians(YagslConstants.kBackLeftEncoderOffset); kBackLeftDriveInvert = YagslConstants.kBackLeftDriveInvert; kBackLeftSteerInvert = YagslConstants.kBackLeftSteerInvert; @@ -233,6 +266,9 @@ public class DriveConstants { kBackRightDriveCanbus = YagslConstants.kBackRightDriveCanbus; kBackRightSteerCanbus = YagslConstants.kBackRightSteerCanbus; kBackRightEncoderCanbus = YagslConstants.kBackRightEncoderCanbus; + kBackRightDriveType = YagslConstants.kBackRightDriveType.toLowerCase(); + kBackRightSteerType = YagslConstants.kBackRightSteerType.toLowerCase(); + kBackRightEncoderType = YagslConstants.kBackRightEncoderType.toLowerCase(); kBackRightEncoderOffset = Units.degreesToRadians(YagslConstants.kBackRightEncoderOffset); kBackRightDriveInvert = YagslConstants.kBackRightDriveInvert; kBackRightSteerInvert = YagslConstants.kBackRightSteerInvert; @@ -271,6 +307,9 @@ public class DriveConstants { kFrontLeftDriveCanbus = ""; kFrontLeftSteerCanbus = ""; kFrontLeftEncoderCanbus = ""; + kFrontLeftDriveType = ""; + kFrontLeftSteerType = ""; + kFrontLeftEncoderType = ""; kFrontLeftEncoderOffset = 0.0; kFrontLeftDriveInvert = false; kFrontLeftSteerInvert = false; @@ -283,6 +322,9 @@ public class DriveConstants { kFrontRightDriveCanbus = ""; kFrontRightSteerCanbus = ""; kFrontRightEncoderCanbus = ""; + kFrontRightDriveType = ""; + kFrontRightSteerType = ""; + kFrontRightEncoderType = ""; kFrontRightEncoderOffset = 0.0; kFrontRightDriveInvert = false; kFrontRightSteerInvert = false; @@ -295,6 +337,9 @@ public class DriveConstants { kBackLeftDriveCanbus = ""; kBackLeftSteerCanbus = ""; kBackLeftEncoderCanbus = ""; + kBackLeftDriveType = ""; + kBackLeftSteerType = ""; + kBackLeftEncoderType = ""; kBackLeftEncoderOffset = 0.0; kBackLeftDriveInvert = false; kBackLeftSteerInvert = false; @@ -307,6 +352,9 @@ public class DriveConstants { kBackRightDriveCanbus = ""; kBackRightSteerCanbus = ""; kBackRightEncoderCanbus = ""; + kBackRightDriveType = ""; + kBackRightSteerType = ""; + kBackRightEncoderType = ""; kBackRightEncoderOffset = 0.0; kBackRightDriveInvert = false; kBackRightSteerInvert = false; @@ -325,4 +373,8 @@ public class DriveConstants { kSteerIZ = 0.0; } } + + // Computed quantities + public static final double kDriveBaseRadius = + Math.hypot(kFrontLeftXPosInches, kFrontLeftYPosInches); } diff --git a/src/main/java/frc/robot/subsystems/drive/README b/src/main/java/frc/robot/subsystems/drive/README index 8efb8405..1e84dc77 100644 --- a/src/main/java/frc/robot/subsystems/drive/README +++ b/src/main/java/frc/robot/subsystems/drive/README @@ -2,7 +2,9 @@ The hardware-specific IO files included here are a baseline for what we expect teams to use. If, however, you are using a different combination of hardware, you will need to create a new ``ModuleIO*.java`` or ``GyroIO*.java`` file. We encourage you to submit a PR to the Az-RBSI repositiory with your new hardware -file if you feel other teams may benefit from its inclusion. +file if you feel other teams may benefit from its inclusion. Additionally, the +instantiation logic in ``Drive.java`` will need to be extended to include the +new module type. Existing IO files: diff --git a/src/main/java/frc/robot/util/YagslConstants.java b/src/main/java/frc/robot/util/YagslConstants.java index 567f15fa..9630b932 100644 --- a/src/main/java/frc/robot/util/YagslConstants.java +++ b/src/main/java/frc/robot/util/YagslConstants.java @@ -126,6 +126,9 @@ public class YagslConstants { public static final String kFrontLeftDriveCanbus = flModule.drive.canbus; public static final String kFrontLeftSteerCanbus = flModule.angle.canbus; public static final String kFrontLeftEncoderCanbus = flModule.encoder.canbus; + public static final String kFrontLeftDriveType = flModule.drive.type; + public static final String kFrontLeftSteerType = flModule.angle.type; + public static final String kFrontLeftEncoderType = flModule.encoder.type; public static final double kFrontLeftEncoderOffset = flModule.absoluteEncoderOffset; public static final boolean kFrontLeftDriveInvert = flModule.inverted.drive; public static final boolean kFrontLeftSteerInvert = flModule.inverted.angle; @@ -141,6 +144,9 @@ public class YagslConstants { public static final String kFrontRightDriveCanbus = frModule.drive.canbus; public static final String kFrontRightSteerCanbus = frModule.angle.canbus; public static final String kFrontRightEncoderCanbus = frModule.encoder.canbus; + public static final String kFrontRightDriveType = frModule.drive.type; + public static final String kFrontRightSteerType = frModule.angle.type; + public static final String kFrontRightEncoderType = frModule.encoder.type; public static final double kFrontRightEncoderOffset = frModule.absoluteEncoderOffset; public static final boolean kFrontRightDriveInvert = frModule.inverted.drive; public static final boolean kFrontRightSteerInvert = frModule.inverted.angle; @@ -156,6 +162,9 @@ public class YagslConstants { public static final String kBackLeftDriveCanbus = blModule.drive.canbus; public static final String kBackLeftSteerCanbus = blModule.angle.canbus; public static final String kBackLeftEncoderCanbus = blModule.encoder.canbus; + public static final String kBackLeftDriveType = blModule.drive.type; + public static final String kBackLeftSteerType = blModule.angle.type; + public static final String kBackLeftEncoderType = blModule.encoder.type; public static final double kBackLeftEncoderOffset = blModule.absoluteEncoderOffset; public static final boolean kBackLeftDriveInvert = blModule.inverted.drive; public static final boolean kBackLeftSteerInvert = blModule.inverted.angle; @@ -171,6 +180,9 @@ public class YagslConstants { public static final String kBackRightDriveCanbus = brModule.drive.canbus; public static final String kBackRightSteerCanbus = brModule.angle.canbus; public static final String kBackRightEncoderCanbus = brModule.encoder.canbus; + public static final String kBackRightDriveType = brModule.drive.type; + public static final String kBackRightSteerType = brModule.angle.type; + public static final String kBackRightEncoderType = brModule.encoder.type; public static final double kBackRightEncoderOffset = brModule.absoluteEncoderOffset; public static final boolean kBackRightDriveInvert = brModule.inverted.drive; public static final boolean kBackRightSteerInvert = brModule.inverted.angle; From 1dd0d00706325bb8e5c324eaaa2c8011a0ad7b29 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 22 Nov 2024 18:26:19 -0700 Subject: [PATCH 51/57] Last-minute thoughts for the nights modified: src/main/deploy/README.md modified: src/main/java/frc/robot/subsystems/drive/DriveConstants.java --- src/main/deploy/README.md | 11 +++ .../subsystems/drive/DriveConstants.java | 84 +------------------ 2 files changed, 12 insertions(+), 83 deletions(-) diff --git a/src/main/deploy/README.md b/src/main/deploy/README.md index d6ec5cf8..88549dc4 100644 --- a/src/main/deploy/README.md +++ b/src/main/deploy/README.md @@ -1,3 +1,14 @@ Files placed in this directory will be deployed to the RoboRIO into the 'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function to get a proper path relative to the deploy directory. + +Currently expected directories: + +apriltags/ + HHH + +pathplanner/ + HHH + +swerve/ + HHH diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 5481bada..da3fce00 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -288,89 +288,7 @@ public class DriveConstants { break; default: - kCoupleRatio = 0.0; - kDriveGearRatio = 0.0; - kSteerGearRatio = 0.0; - kWheelRadiusInches = 0.0; - kCANbusName = ""; - kPigeonId = 0; - kSteerInertia = 0.0; - kDriveInertia = 0.0; - kSteerFrictionVoltage = 0.0; - kDriveFrictionVoltage = 0.0; - kSteerCurrentLimit = 0.0; - kDriveCurrentLimit = 0.0; - kOptimalVoltage = 0.0; - kFrontLeftDriveMotorId = 0; - kFrontLeftSteerMotorId = 0; - kFrontLeftEncoderId = 0; - kFrontLeftDriveCanbus = ""; - kFrontLeftSteerCanbus = ""; - kFrontLeftEncoderCanbus = ""; - kFrontLeftDriveType = ""; - kFrontLeftSteerType = ""; - kFrontLeftEncoderType = ""; - kFrontLeftEncoderOffset = 0.0; - kFrontLeftDriveInvert = false; - kFrontLeftSteerInvert = false; - kFrontLeftEncoderInvert = false; - kFrontLeftXPosInches = 0.0; - kFrontLeftYPosInches = 0.0; - kFrontRightDriveMotorId = 0; - kFrontRightSteerMotorId = 0; - kFrontRightEncoderId = 0; - kFrontRightDriveCanbus = ""; - kFrontRightSteerCanbus = ""; - kFrontRightEncoderCanbus = ""; - kFrontRightDriveType = ""; - kFrontRightSteerType = ""; - kFrontRightEncoderType = ""; - kFrontRightEncoderOffset = 0.0; - kFrontRightDriveInvert = false; - kFrontRightSteerInvert = false; - kFrontRightEncoderInvert = false; - kFrontRightXPosInches = 0.0; - kFrontRightYPosInches = 0.0; - kBackLeftDriveMotorId = 0; - kBackLeftSteerMotorId = 0; - kBackLeftEncoderId = 0; - kBackLeftDriveCanbus = ""; - kBackLeftSteerCanbus = ""; - kBackLeftEncoderCanbus = ""; - kBackLeftDriveType = ""; - kBackLeftSteerType = ""; - kBackLeftEncoderType = ""; - kBackLeftEncoderOffset = 0.0; - kBackLeftDriveInvert = false; - kBackLeftSteerInvert = false; - kBackLeftEncoderInvert = false; - kBackLeftXPosInches = 0.0; - kBackLeftYPosInches = 0.0; - kBackRightDriveMotorId = 0; - kBackRightSteerMotorId = 0; - kBackRightEncoderId = 0; - kBackRightDriveCanbus = ""; - kBackRightSteerCanbus = ""; - kBackRightEncoderCanbus = ""; - kBackRightDriveType = ""; - kBackRightSteerType = ""; - kBackRightEncoderType = ""; - kBackRightEncoderOffset = 0.0; - kBackRightDriveInvert = false; - kBackRightSteerInvert = false; - kBackRightEncoderInvert = false; - kBackRightXPosInches = 0.0; - kBackRightYPosInches = 0.0; - kDriveP = 0.0; - kDriveI = 0.0; - kDriveD = 0.0; - kDriveF = 0.0; - kDriveIZ = 0.0; - kSteerP = 0.0; - kSteerI = 0.0; - kSteerD = 0.0; - kSteerF = 0.0; - kSteerIZ = 0.0; + throw new RuntimeException("Invalid Swerve Drive Type"); } } From ae974fbb6768ed316d7b0bbb12e34bc612d199aa Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Mon, 25 Nov 2024 11:25:08 -0700 Subject: [PATCH 52/57] Add AK-compliant commands; TESTING REQ'D The thing finally builds without errors. TESTING REQUIRED before Alpha Release! modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/commands/DriveCommands.java modified: src/main/java/frc/robot/subsystems/drive/Drive.java modified: src/main/java/frc/robot/subsystems/drive/GyroIO.java modified: src/main/java/frc/robot/subsystems/drive/GyroIONavX.java modified: src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java --- src/main/java/frc/robot/Constants.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 210 +++++++++++------- .../frc/robot/commands/DriveCommands.java | 87 ++++++-- .../frc/robot/subsystems/drive/Drive.java | 134 ++++++----- .../frc/robot/subsystems/drive/GyroIO.java | 2 + .../robot/subsystems/drive/GyroIONavX.java | 28 +++ .../robot/subsystems/drive/GyroIOPigeon2.java | 21 ++ 7 files changed, 328 insertions(+), 160 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 728da6fb..3e4e6204 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -266,10 +266,8 @@ public static final class FlywheelConstants { public static class OperatorConstants { // Joystick Deadband - public static final double kLeftXDeadband = 0.1; - public static final double kLeftYDeadband = 0.1; - public static final double kRightXDeadband = 0.1; - public static final double kRightYDeadband = 0.1; + public static final double kLeftDeadband = 0.1; + public static final double kRightDeadband = 0.1; public static final double kTurnConstant = 6; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 507bcec2..95bfe3de 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,24 +27,30 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.AprilTagConstants; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; import frc.robot.Constants.AutoConstants; -import frc.robot.Constants.DrivebaseConstants; +import frc.robot.commands.DriveCommands; import frc.robot.subsystems.accelerometer.Accelerometer; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.flywheel_example.Flywheel; import frc.robot.subsystems.flywheel_example.FlywheelIO; import frc.robot.subsystems.flywheel_example.FlywheelIOSim; -import frc.robot.subsystems.swervedrive_ignore.SwerveTelemetry; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhoton; +import frc.robot.util.Alert; +import frc.robot.util.Alert.AlertType; +import frc.robot.util.LoggedTunableNumber; import frc.robot.util.OverrideSwitches; import frc.robot.util.PowerMonitoring; import frc.robot.util.RobotDeviceId; @@ -71,9 +77,12 @@ public class RobotContainer { // Dashboard inputs private final LoggedDashboardChooser autoChooser; + // EXAMPLE TUNABLE FLYWHEEL SPEED INPUT FROM DASHBOARD + private final LoggedTunableNumber flywheelSpeedInput = + new LoggedTunableNumber("Flywheel Speed", 1500.0); - // Swerve Drive Telemetry - private final SwerveTelemetry logger = new SwerveTelemetry(DrivebaseConstants.kMaxLinearSpeed); + // Alerts + private final Alert aprilTagLayoutAlert = new Alert("", AlertType.INFO); /** Returns the current AprilTag layout type. */ public AprilTagLayoutType getAprilTagLayoutType() { @@ -131,71 +140,95 @@ public RobotContainer() { configureBindings(); // Define Auto commands defineAutoCommands(); + // Define SysIs Routines + definesysIdRoutines(); // Set up the SmartDashboard Auto Chooser autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); - // Set up the logger - m_drivebase.registerTelemetry(logger::telemeterize); } /** Use this method to define your Autonomous commands for use with PathPlanner / Choreo */ private void defineAutoCommands() { - NamedCommands.registerCommand("Zero", Commands.runOnce(() -> m_drivebase.tareEverything())); + NamedCommands.registerCommand("Zero", Commands.runOnce(() -> m_drivebase.zero())); + } + + /** Set up the SysID routines from AdvantageKit */ + private void definesysIdRoutines() { + // Drivebase characterization + autoChooser.addOption( + "Drive SysId (Quasistatic Forward)", + m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive SysId (Quasistatic Reverse)", + m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Drive SysId (Dynamic Forward)", m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive SysId (Dynamic Reverse)", m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + + // Example Flywheel SysId Characterization + autoChooser.addOption( + "Flywheel SysId (Quasistatic Forward)", + m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Flywheel SysId (Quasistatic Reverse)", + m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Flywheel SysId (Dynamic Forward)", + m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Flywheel SysId (Dynamic Reverse)", + m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); } /** - * Use this method to define your trigger->command mappings. Triggers can be created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary - * predicate, or via the named factories in {@link - * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link - * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller - * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight - * joysticks}. + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureBindings() { - // Manually Re-Zero the Gyro & Odometry - driverXbox.y().onTrue(Commands.runOnce(() -> m_drivebase.tareEverything())); - - // Example Swerve Drive button bindings - // A -> BRAKE - driverXbox.a().whileTrue(m_drivebase.applyRequest(() -> m_drivebase.brake)); - // B -> POINT WHEELS AT DIRECTION WITHOUT MOVING + // SET STANDARD DRIVING AS DEFAULT COMMAND FOR THE DRIVEBASE + // NOTE: Left joystick controls lateral translation, right joystick (X) controls rotation + m_drivebase.setDefaultCommand( + DriveCommands.fieldRelativeDrive( + m_drivebase, + () -> -driverXbox.getRightY(), + () -> -driverXbox.getRightX(), + () -> -driverXbox.getLeftX())); + + // Example Commands + // Press B button while driving --> ROBOT-CENTRIC driverXbox .b() - .whileTrue( - m_drivebase.applyRequest( + .onTrue( + Commands.runOnce( () -> - m_drivebase.point.withModuleDirection( - new Rotation2d(-driverXbox.getLeftY(), -driverXbox.getLeftX())))); - // LEFT BUMPER -> reset the field-centric heading - driverXbox.leftBumper().onTrue(m_drivebase.runOnce(() -> m_drivebase.seedFieldRelative())); - // POV UP -> DRIVE FORWARD IN ROBOT-CENTRIC MODE - driverXbox - .pov(0) - .whileTrue( - m_drivebase.applyRequest( - () -> m_drivebase.forwardStraight.withVelocityX(0.5).withVelocityY(0))); - // POV DOWN -> DRIVE BACKWARD IN ROBOT-CENTRIC MODE + DriveCommands.robotRelativeDrive( + m_drivebase, + () -> -driverXbox.getRightY(), + () -> -driverXbox.getRightX(), + () -> -driverXbox.getLeftX()), + m_drivebase)); + + // Press A button -> BRAKE + driverXbox.a().whileTrue(Commands.runOnce(() -> m_drivebase.setMotorBrake(true), m_drivebase)); + + // Press X button --> Stop with wheels in X-Lock position + driverXbox.x().onTrue(Commands.runOnce(m_drivebase::stopWithX, m_drivebase)); + + // Press Y button --> Manually Re-Zero the Gyro + driverXbox.y().onTrue(Commands.runOnce(() -> m_drivebase.zero())); + + // Press RIGHT BUMPER --> Run the example flywheel driverXbox - .pov(180) + .rightBumper() .whileTrue( - m_drivebase.applyRequest( - () -> m_drivebase.forwardStraight.withVelocityX(-0.5).withVelocityY(0))); - - // SET STANDARD DRIVING AS DEFAULT COMMAND FOR THE DRIVEBASE - // NOTE: Left joystick controls lateral translation, right joystick (X) controls rotation - m_drivebase.setDefaultCommand( - m_drivebase - .applyRequest( - () -> - m_drivebase - .drive - .withVelocityX(-driverXbox.getLeftY() * DrivebaseConstants.kMaxLinearSpeed) - .withVelocityY(-driverXbox.getLeftX() * DrivebaseConstants.kMaxLinearSpeed) - .withRotationalRate( - -driverXbox.getRightX() * DrivebaseConstants.kMaxAngularSpeed)) - .ignoringDisable(true)); + Commands.startEnd( + () -> m_flywheel.runVelocity(flywheelSpeedInput.get()), + m_flywheel::stop, + m_flywheel)); } /** @@ -220,44 +253,44 @@ public Command getAutonomousCommandChoreo() { var thetaController = new PIDController(AutoConstants.kPThetaController, 0, 0); thetaController.enableContinuousInput(-Math.PI, Math.PI); - m_drivebase.resetOdometry(traj.getInitialPose()); + m_drivebase.setPose(traj.getInitialPose()); + + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; Command swerveCommand = Choreo.choreoSwerveCommand( - traj, // Choreo trajectory from above - m_drivebase - ::getPose, // A function that returns the current field-relative pose of the robot: - // your - // wheel or vision odometry - new PIDController( - Constants.AutoConstants.kPXController, - 0.0, - 0.0), // PIDController for field-relative X - // translation (input: X error in meters, - // output: m/s). - new PIDController( - Constants.AutoConstants.kPYController, - 0.0, - 0.0), // PIDController for field-relative Y - // translation (input: Y error in meters, - // output: m/s). - thetaController, // PID constants to correct for rotation - // error + // Choreo trajectory from above + traj, + // A function that returns the current field-relative pose of the robot: your wheel or + // vision odometry + m_drivebase::getPose, + // PIDController for field-relative X translation (input: X error meters, output: m/s). + new PIDController(Constants.AutoConstants.kPXController, 0.0, 0.0), + // PIDController for field-relative Y translation (input: Y error meters, output: m/s). + new PIDController(Constants.AutoConstants.kPYController, 0.0, 0.0), + // PID constants to correct for rotation error + thetaController, (ChassisSpeeds speeds) -> - m_drivebase.drive( // needs to be robot-relative - speeds.vxMetersPerSecond, - speeds.vyMetersPerSecond, - speeds.omegaRadiansPerSecond, - false), - true, // Whether or not to mirror the path based on alliance (this assumes the path is - // created for the blue alliance) - m_drivebase // The subsystem(s) to require, typically your drive subsystem only - ); + m_drivebase.runVelocity( + ChassisSpeeds.fromRobotRelativeSpeeds( + speeds.vxMetersPerSecond, + speeds.vyMetersPerSecond, + speeds.omegaRadiansPerSecond, + isFlipped + ? m_drivebase.getRotation().plus(new Rotation2d(Math.PI)) + : m_drivebase.getRotation())), + // Whether or not to mirror the path based on alliance (this assumes the path is created + // for the blue alliance) + () -> true, + // The subsystem(s) to require, typically your drive subsystem only + m_drivebase); return Commands.sequence( - Commands.runOnce(() -> m_drivebase.resetOdometry(traj.getInitialPose())), + Commands.runOnce(() -> m_drivebase.setPose(traj.getInitialPose())), swerveCommand, - m_drivebase.run(() -> m_drivebase.drive(0, 0, 0, false))); + m_drivebase.run(() -> m_drivebase.stop())); } public void setDriveMode() { @@ -269,6 +302,17 @@ public void setMotorBrake(boolean brake) { m_drivebase.setMotorBrake(brake); } + /** Updates the alerts. */ + public void updateAlerts() { + // AprilTag layout alert + boolean aprilTagAlertActive = getAprilTagLayoutType() != AprilTagLayoutType.OFFICIAL; + aprilTagLayoutAlert.set(aprilTagAlertActive); + if (aprilTagAlertActive) { + aprilTagLayoutAlert.setText( + "Non-official AprilTag layout in use (" + getAprilTagLayoutType().toString() + ")."); + } + } + /** List of Device CAN and Power Distribution Circuit IDs **************** */ public static class Ports { diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 7ed33afa..35356f09 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -25,41 +25,27 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.drive.Drive; import java.util.function.DoubleSupplier; public class DriveCommands { - private static final double DEADBAND = 0.1; private DriveCommands() {} /** * Field relative drive command using two joysticks (controlling linear and angular velocities). */ - public static Command joystickDrive( + public static Command fieldRelativeDrive( Drive drive, DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier) { return Commands.run( () -> { - // Apply deadband - double linearMagnitude = - MathUtil.applyDeadband( - Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND); - Rotation2d linearDirection = - new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); - double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); - - // Square values - linearMagnitude = linearMagnitude * linearMagnitude; - omega = Math.copySign(omega * omega, omega); - - // Calcaulate new linear velocity - Translation2d linearVelocity = - new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) - .getTranslation(); + // Get the Linear Velocity & Omega from inputs + Translation2d linearVelocity = getLinearVelocity(xSupplier, ySupplier); + double omega = getOmega(omegaSupplier); // Convert to field relative speeds & send command boolean isFlipped = @@ -76,4 +62,67 @@ public static Command joystickDrive( }, drive); } + + /** + * Robot relative drive command using two joysticks (controlling linear and angular velocities). + */ + public static Command robotRelativeDrive( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier) { + return Commands.run( + () -> { + + // Get the Linear Velocity & Omega from inputs + Translation2d linearVelocity = getLinearVelocity(xSupplier, ySupplier); + double omega = getOmega(omegaSupplier); + + // Convert to robot relative speeds & send command + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity( + ChassisSpeeds.fromRobotRelativeSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega * drive.getMaxAngularSpeedRadPerSec(), + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); + }, + drive); + } + + /** + * Compute the new linear velocity from inputs, including applying deadbands and squaring for + * smoothness + */ + private static Translation2d getLinearVelocity( + DoubleSupplier xSupplier, DoubleSupplier ySupplier) { + + // Apply deadband + double linearMagnitude = + MathUtil.applyDeadband( + Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), + OperatorConstants.kLeftDeadband); + Rotation2d linearDirection = new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + + // Square values + linearMagnitude = linearMagnitude * linearMagnitude; + + return new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + } + + /** + * Compute the new angular velocity from inputs, including applying deadbands and squaring for + * smoothness + */ + private static double getOmega(DoubleSupplier omegaSupplier) { + double omega = + MathUtil.applyDeadband(omegaSupplier.getAsDouble(), OperatorConstants.kRightDeadband); + return Math.copySign(omega * omega, omega); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 3a963157..bdce538c 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -61,7 +61,7 @@ public class Drive extends SubsystemBase { new SwerveModulePosition(), new SwerveModulePosition() }; - private SwerveDrivePoseEstimator poseEstimator = + private SwerveDrivePoseEstimator m_PoseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); // Constructor @@ -88,55 +88,8 @@ public Drive() { default: throw new RuntimeException("Invalid IMU type"); } - // Then choose the module(s) - // NOTE: This assumes all 4 modules have the same arrangement! - Byte b_drive; // [x,x,-,-,-,-,-,-] - Byte b_steer; // [-,-,x,x,-,-,-,-] - Byte b_encoder; // [-,-,-,-,x,x,-,-] - switch (kFrontLeftDriveType) { - case "falcon": - case "kraken": - case "talonfx": - // CTRE Drive Motor - b_drive = 0b00; - break; - case "sparkmax": - case "sparkflex": - // REV Drive Motor - b_drive = 0b01; - break; - default: - throw new RuntimeException("Invalid drive motor type"); - } - switch (kFrontLeftSteerType) { - case "falcon": - case "kraken": - case "talonfx": - // CTRE Drive Motor - b_steer = 0b00; - break; - case "sparkmax": - case "sparkflex": - // REV Drive Motor - b_steer = 0b01; - break; - default: - throw new RuntimeException("Invalid steer motor type"); - } - switch (kFrontLeftEncoderType) { - case "cancoder": - // CTRE CANcoder - b_encoder = 0b00; - break; - case "analog": - // Analog Encoder - b_encoder = 0b01; - break; - default: - throw new RuntimeException("Invalid swerve encoder type"); - } - Byte modType = (byte) (0b00000000 | b_drive << 6 | b_steer << 4 | b_encoder << 2); - + // Then parse the module(s) + Byte modType = parseModuleType(); for (int i = 0; i < 4; i++) { switch (modType) { case 0b00000000: // ALL-CTRE @@ -245,7 +198,20 @@ public void periodic() { } // Apply odometry update - poseEstimator.update(rawGyroRotation, modulePositions); + m_PoseEstimator.update(rawGyroRotation, modulePositions); + } + + /** + * Sets the swerve drive motors to brake/coast mode. + * + * @param brake True to set motors to brake mode, false for coast. + */ + public void setMotorBrake(boolean brake) { + { + for (Module swerveModule : modules) { + swerveModule.setBrakeMode(brake); + } + } } /** @@ -271,6 +237,11 @@ public void runVelocity(ChassisSpeeds speeds) { Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); } + /** Re-zero the gyro at the present heading */ + public void zero() { + gyroIO.zero(); + } + /** Stops the drive. */ public void stop() { runVelocity(new ChassisSpeeds()); @@ -321,7 +292,7 @@ private SwerveModulePosition[] getModulePositions() { /** Returns the current odometry pose. */ @AutoLogOutput(key = "Odometry/Robot") public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); + return m_PoseEstimator.getEstimatedPosition(); } /** Returns the current odometry rotation. */ @@ -331,7 +302,7 @@ public Rotation2d getRotation() { /** Resets the current odometry pose. */ public void setPose(Pose2d pose) { - poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + m_PoseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); } /** @@ -341,7 +312,7 @@ public void setPose(Pose2d pose) { * @param timestamp The timestamp of the vision measurement in seconds. */ public void addVisionMeasurement(Pose2d visionPose, double timestamp) { - poseEstimator.addVisionMeasurement(visionPose, timestamp); + m_PoseEstimator.addVisionMeasurement(visionPose, timestamp); } /** Returns the maximum linear speed in meters per sec. */ @@ -367,4 +338,59 @@ public static Translation2d[] getModuleTranslations() { public T getGyro() { return gyroIO.getGyro(); } + + /** + * Parse the module type given the type information for the FL module + * + * @return Byte The module type as bits in a byte. + */ + private Byte parseModuleType() { + // NOTE: This assumes all 4 modules have the same arrangement! + Byte b_drive; // [x,x,-,-,-,-,-,-] + Byte b_steer; // [-,-,x,x,-,-,-,-] + Byte b_encoder; // [-,-,-,-,x,x,-,-] + switch (kFrontLeftDriveType) { + case "falcon": + case "kraken": + case "talonfx": + // CTRE Drive Motor + b_drive = 0b00; + break; + case "sparkmax": + case "sparkflex": + // REV Drive Motor + b_drive = 0b01; + break; + default: + throw new RuntimeException("Invalid drive motor type"); + } + switch (kFrontLeftSteerType) { + case "falcon": + case "kraken": + case "talonfx": + // CTRE Drive Motor + b_steer = 0b00; + break; + case "sparkmax": + case "sparkflex": + // REV Drive Motor + b_steer = 0b01; + break; + default: + throw new RuntimeException("Invalid steer motor type"); + } + switch (kFrontLeftEncoderType) { + case "cancoder": + // CTRE CANcoder + b_encoder = 0b00; + break; + case "analog": + // Analog Encoder + b_encoder = 0b01; + break; + default: + throw new RuntimeException("Invalid swerve encoder type"); + } + return (byte) (0b00000000 | b_drive << 6 | b_steer << 4 | b_encoder << 2); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 10c8c3ec..6a4f2861 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -28,5 +28,7 @@ public static class GyroIOInputs { public default void updateInputs(GyroIOInputs inputs) {} + public default void zero() {} + public abstract T getGyro(); } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index 0b8c8364..40b4709b 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -18,6 +18,8 @@ import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.SPI; /** IO implementation for Pigeon2 */ @@ -28,6 +30,12 @@ public class GyroIONavX implements GyroIO { public GyroIONavX() { navx = new AHRS(SPI.Port.kMXP, (byte) 100.0); navx.reset(); + // Set Angle Adjustment based on alliance + if (DriverStation.getAlliance().get() == Alliance.Blue) { + navx.setAngleAdjustment(0.0); + } else { + navx.setAngleAdjustment(180.0); + } } // Return the Pigeon2 instance @@ -42,4 +50,24 @@ public void updateInputs(GyroIOInputs inputs) { inputs.yawPosition = Rotation2d.fromDegrees(navx.getYaw()); inputs.yawVelocityRadPerSec = Units.degreesToRadians(navx.getRate()); } + + /** + * Zero the NavX + * + *

This method should always rezero the pigeon in ALWAYS-BLUE-ORIGIN orientation. Testing, + * however, shows that it's not doing what I think it should be doing. There is likely + * interference with something else in the odometry + */ + @Override + public void zero() { + // With the Pigeon facing forward, forward depends on the alliance selected. + // Set Angle Adjustment based on alliance + if (DriverStation.getAlliance().get() == Alliance.Blue) { + navx.setAngleAdjustment(0.0); + } else { + navx.setAngleAdjustment(180.0); + } + System.out.println("Setting YAW to " + navx.getAngleAdjustment()); + navx.zeroYaw(); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 1caa44d1..28a27a4d 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -24,6 +24,8 @@ import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; /** IO implementation for Pigeon2 */ public class GyroIOPigeon2 implements GyroIO { @@ -56,4 +58,23 @@ public void updateInputs(GyroIOInputs inputs) { inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); } + + /** + * Zero the Pigeon2 + * + *

This method should always rezero the pigeon in ALWAYS-BLUE-ORIGIN orientation. Testing, + * however, shows that it's not doing what I think it should be doing. There is likely + * interference with something else in the odometry + */ + @Override + public void zero() { + // With the Pigeon facing forward, forward depends on the alliance selected. + if (DriverStation.getAlliance().get() == Alliance.Blue) { + System.out.println("Alliance Blue: Setting YAW to 0"); + pigeon.setYaw(0.0); + } else { + System.out.println("Alliance Red: Setting YAW to 180"); + pigeon.setYaw(180.0); + } + } } From 642ab3217e0e64aaee7b6f2f539b5ee3d972c329 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Sun, 1 Dec 2024 04:18:59 +0000 Subject: [PATCH 53/57] Bump io.freefair.lombok from 8.10.2 to 8.11 Bumps io.freefair.lombok from 8.10.2 to 8.11. --- updated-dependencies: - dependency-name: io.freefair.lombok dependency-type: direct:production update-type: version-update:semver-minor ... Signed-off-by: dependabot[bot] --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index 68225681..1a1866a6 100644 --- a/build.gradle +++ b/build.gradle @@ -3,7 +3,7 @@ plugins { id "edu.wpi.first.GradleRIO" version "2024.3.2" id "com.peterabeles.gversion" version "1.10.3" id "com.diffplug.spotless" version "6.25.0" - id "io.freefair.lombok" version "8.10.2" + id "io.freefair.lombok" version "8.11" id "com.google.protobuf" version "0.9.4" } From 06e27c8d1849d16a7dc4423a314d59ba10285e47 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sat, 30 Nov 2024 22:43:10 -0700 Subject: [PATCH 54/57] Code builds and deploys; bugs to be fixed Issues: * Power monitoring not working at all * CAN issues -- maybe Az-RBSI, maybe my drivebase * Add Constant value for whether PhotonVision is enabled modified: build.gradle modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/RobotContainer.java deleted: src/main/java/frc/robot/commands/swervedrive/auto/YAGSL_AutoBalanceCommand.java deleted: src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDrive.java deleted: src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java deleted: src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteFieldDrive.java modified: src/main/java/frc/robot/generated/TunerConstants.java modified: src/main/java/frc/robot/subsystems/drive/Drive.java modified: src/main/java/frc/robot/subsystems/drive/GyroIO.java modified: src/main/java/frc/robot/subsystems/drive/GyroIONavX.java modified: src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java modified: src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java modified: src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java --- build.gradle | 2 +- src/main/java/frc/robot/Constants.java | 3 +- src/main/java/frc/robot/Robot.java | 13 +- src/main/java/frc/robot/RobotContainer.java | 24 +- .../auto/YAGSL_AutoBalanceCommand.java | 102 --- .../drivebase/YAGSL_AbsoluteDrive.java | 140 ---- .../drivebase/YAGSL_AbsoluteDriveAdv.java | 171 ---- .../drivebase/YAGSL_AbsoluteFieldDrive.java | 107 --- .../frc/robot/generated/TunerConstants.java | 7 +- .../frc/robot/subsystems/drive/Drive.java | 59 +- .../frc/robot/subsystems/drive/GyroIO.java | 4 +- .../robot/subsystems/drive/GyroIONavX.java | 4 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 4 +- .../swervedrive_ignore/SwerveSubsystem.java | 756 +++++++++--------- .../swervedrive_ignore/SwerveTelemetry.java | 293 +++---- 15 files changed, 601 insertions(+), 1088 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/swervedrive/auto/YAGSL_AutoBalanceCommand.java delete mode 100644 src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDrive.java delete mode 100644 src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java delete mode 100644 src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteFieldDrive.java diff --git a/build.gradle b/build.gradle index 1a1866a6..14021685 100644 --- a/build.gradle +++ b/build.gradle @@ -135,7 +135,7 @@ wpi.java.configureTestTasks(test) // Configure string concat to always inline compile tasks.withType(JavaCompile) { - options.compilerArgs.add '-XDstringConcat=inline' + options.compilerArgs << '-XDstringConcat=inline' << '-Xlint:unchecked' } // Create version file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3e4e6204..bf06caaa 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -52,10 +52,9 @@ public final class Constants { * Define the various multiple robots that use this same code (e.g., COMPBOT, DEVBOT, SIMBOT, * etc.) and the operating modes of the code (REAL, SIM, or REPLAY) */ - private static RobotType robotType = RobotType.SIMBOT; + private static RobotType robotType = RobotType.COMPBOT; private static SwerveType swerveType = SwerveType.PHOENIX6; - private static AutoType autoType = AutoType.PATHPLANNER; public static boolean disableHAL = false; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fdb1de8b..6cd3c0c0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -144,8 +144,17 @@ public void disabledPeriodic() { @Override public void autonomousInit() { m_robotContainer.setMotorBrake(true); - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - + switch (Constants.getAutoType()) { + case PATHPLANNER: + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + break; + case CHOREO: + m_autonomousCommand = m_robotContainer.getAutonomousCommandChoreo(); + break; + default: + throw new RuntimeException( + "Incorrect AUTO type selected in Constants: " + Constants.getAutoType()); + } // schedule the autonomous command (example) if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 95bfe3de..1fbaebac 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -65,8 +65,9 @@ public class RobotContainer { final CommandXboxController operatorXbox = new CommandXboxController(1); final OverrideSwitches overrides = new OverrideSwitches(2); + // Autonomous Things Field2d m_field = new Field2d(); - ChoreoTrajectory traj; + ChoreoTrajectory m_traj; // Declare the robot subsystems here private final Drive m_drivebase; @@ -95,10 +96,6 @@ public static AprilTagLayoutType staticGetAprilTagLayoutType() { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - traj = Choreo.getTrajectory("Trajectory"); - - m_field.getObject("traj").setPoses(traj.getInitialPose(), traj.getFinalPose()); - m_field.getObject("trajPoses").setPoses(traj.getPoses()); // Instantiate Robot Subsystems based on RobotType switch (Constants.getMode()) { @@ -136,14 +133,15 @@ public RobotContainer() { // as that is automatically monitored. m_power = new PowerMonitoring(m_flywheel); + // Set up the SmartDashboard Auto Chooser + autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); + // Configure the trigger bindings configureBindings(); // Define Auto commands defineAutoCommands(); // Define SysIs Routines definesysIdRoutines(); - // Set up the SmartDashboard Auto Chooser - autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); } /** Use this method to define your Autonomous commands for use with PathPlanner / Choreo */ @@ -250,10 +248,16 @@ public Command getAutonomousCommand() { * @return the command to run in autonomous */ public Command getAutonomousCommandChoreo() { + + m_traj = Choreo.getTrajectory("Trajectory"); + + m_field.getObject("traj").setPoses(m_traj.getInitialPose(), m_traj.getFinalPose()); + m_field.getObject("trajPoses").setPoses(m_traj.getPoses()); + var thetaController = new PIDController(AutoConstants.kPThetaController, 0, 0); thetaController.enableContinuousInput(-Math.PI, Math.PI); - m_drivebase.setPose(traj.getInitialPose()); + m_drivebase.setPose(m_traj.getInitialPose()); boolean isFlipped = DriverStation.getAlliance().isPresent() @@ -262,7 +266,7 @@ public Command getAutonomousCommandChoreo() { Command swerveCommand = Choreo.choreoSwerveCommand( // Choreo trajectory from above - traj, + m_traj, // A function that returns the current field-relative pose of the robot: your wheel or // vision odometry m_drivebase::getPose, @@ -288,7 +292,7 @@ public Command getAutonomousCommandChoreo() { m_drivebase); return Commands.sequence( - Commands.runOnce(() -> m_drivebase.setPose(traj.getInitialPose())), + Commands.runOnce(() -> m_drivebase.setPose(m_traj.getInitialPose())), swerveCommand, m_drivebase.run(() -> m_drivebase.stop())); } diff --git a/src/main/java/frc/robot/commands/swervedrive/auto/YAGSL_AutoBalanceCommand.java b/src/main/java/frc/robot/commands/swervedrive/auto/YAGSL_AutoBalanceCommand.java deleted file mode 100644 index fa2f309c..00000000 --- a/src/main/java/frc/robot/commands/swervedrive/auto/YAGSL_AutoBalanceCommand.java +++ /dev/null @@ -1,102 +0,0 @@ -// // Copyright (c) 2024 Az-FIRST -// // http://github.com/AZ-First -// // -// // This program is free software; you can redistribute it and/or -// // modify it under the terms of the GNU General Public License -// // version 3 as published by the Free Software Foundation or -// // available in the root directory of this project. -// // -// // 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. -// // -// // Copyright (c) FIRST and other WPILib contributors. -// // Open Source Software; you can modify and/or share it under the terms of -// // the WPILib BSD license file in the root directory of this project. -// // -// // NOTE: This module from the YAGSL Example Project - -// package frc.robot.commands.swervedrive.auto; - -// import edu.wpi.first.math.MathUtil; -// import edu.wpi.first.math.controller.PIDController; -// import edu.wpi.first.math.geometry.Translation2d; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; - -// /** -// * Auto Balance command using a simple PID controller. Created by Team 3512 ... -// */ -// public class YAGSL_AutoBalanceCommand extends Command { - -// private final YAGSLSwerve swerveSubsystem; -// private final PIDController controller; - -// public YAGSL_AutoBalanceCommand(YAGSLSwerve swerveSubsystem) { -// this.swerveSubsystem = swerveSubsystem; -// controller = new PIDController(1.0, 0.0, 0.0); -// controller.setTolerance(1); -// controller.setSetpoint(0.0); -// // each subsystem used by the command must be passed into the -// // addRequirements() method (which takes a vararg of Subsystem) -// addRequirements(this.swerveSubsystem); -// } - -// /** The initial subroutine of a command. Called once when the command is initially scheduled. -// */ -// @Override -// public void initialize() {} - -// /** -// * The main body of a command. Called repeatedly while the command is scheduled. (That is, it -// is -// * called repeatedly until {@link #isFinished()}) returns true.) -// */ -// @Override -// public void execute() { -// SmartDashboard.putBoolean("At Tolerance", controller.atSetpoint()); - -// double translationVal = -// MathUtil.clamp( -// controller.calculate(swerveSubsystem.getPitch().getDegrees(), 0.0), -0.5, 0.5); -// swerveSubsystem.drive(new Translation2d(translationVal, 0.0), 0.0, true); -// } - -// /** -// * Returns whether this command has finished. Once a command finishes -- indicated by this -// method -// * returning true -- the scheduler will call its {@link #end(boolean)} method. -// * -// *

Returning false will result in the command never ending automatically. It may still be -// * cancelled manually or interrupted by another command. Hard coding this command to always -// return -// * true will result in the command executing once and finishing immediately. It is recommended -// to -// * use * {@link edu.wpi.first.wpilibj2.command.InstantCommand InstantCommand} for such an -// * operation. -// * -// * @return whether this command has finished. -// */ -// @Override -// public boolean isFinished() { -// return controller.atSetpoint(); -// } - -// /** -// * The action to take when the command ends. Called when either the command finishes normally -// -- -// * that is it is called when {@link #isFinished()} returns true -- or when it is -// * interrupted/canceled. This is where you may want to wrap up loose ends, like shutting off a -// * motor that was being used in the command. -// * -// * @param interrupted whether the command was interrupted/canceled -// */ -// @Override -// public void end(boolean interrupted) { -// swerveSubsystem.lock(); -// } -// } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDrive.java deleted file mode 100644 index da035d86..00000000 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDrive.java +++ /dev/null @@ -1,140 +0,0 @@ -// // Copyright (c) 2024 Az-FIRST -// // http://github.com/AZ-First -// // -// // This program is free software; you can redistribute it and/or -// // modify it under the terms of the GNU General Public License -// // version 3 as published by the Free Software Foundation or -// // available in the root directory of this project. -// // -// // 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. -// // -// // Copyright (c) FIRST and other WPILib contributors. -// // Open Source Software; you can modify and/or share it under the terms of -// // the WPILib BSD license file in the root directory of this project. -// // -// // NOTE: This module from the YAGSL Example Project - -// package frc.robot.commands.swervedrive.drivebase; - -// import edu.wpi.first.math.geometry.Rotation2d; -// import edu.wpi.first.math.geometry.Translation2d; -// import edu.wpi.first.math.kinematics.ChassisSpeeds; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.Constants.PhysicalConstants; -// import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; -// import java.util.List; -// import java.util.function.DoubleSupplier; -// import swervelib.SwerveController; -// import swervelib.math.SwerveMath; - -// /** An example command that uses an example subsystem. */ -// public class YAGSL_AbsoluteDrive extends Command { - -// private final YAGSLSwerve swerve; -// private final DoubleSupplier vX, vY; -// private final DoubleSupplier headingHorizontal, headingVertical; -// private boolean initRotation = false; - -// /** -// * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation -// inputs, -// * where x is torwards/away from alliance wall and y is left/right. headingHorzontal and -// * headingVertical are the Cartesian coordinates from which the robot's angle will be derived— -// * they will be converted to a polar angle, which the robot will rotate to. -// * -// * @param swerve The swerve drivebase subsystem. -// * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the -// range -// * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. -// * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the -// range -// * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when -// * looking through the driver station glass. -// * @param headingHorizontal DoubleSupplier that supplies the horizontal component of the -// robot's -// * heading angle. In the robot coordinate system, this is along the same axis as vY. Should -// * range from -1 to 1 with no deadband. Positive is towards the left wall when looking -// through -// * the driver station glass. -// * @param headingVertical DoubleSupplier that supplies the vertical component of the robot's -// * heading angle. In the robot coordinate system, this is along the same axis as vX. Should -// * range from -1 to 1 with no deadband. Positive is away from the alliance wall. -// */ -// public YAGSL_AbsoluteDrive( -// YAGSLSwerve swerve, -// DoubleSupplier vX, -// DoubleSupplier vY, -// DoubleSupplier headingHorizontal, -// DoubleSupplier headingVertical) { -// this.swerve = swerve; -// this.vX = vX; -// this.vY = vY; -// this.headingHorizontal = headingHorizontal; -// this.headingVertical = headingVertical; - -// addRequirements(swerve); -// } - -// @Override -// public void initialize() { -// initRotation = true; -// } - -// // Called every time the scheduler runs while the command is scheduled. -// @Override -// public void execute() { - -// // Get the desired chassis speeds based on a 2 joystick module. -// ChassisSpeeds desiredSpeeds = -// swerve.getTargetSpeeds( -// vX.getAsDouble(), -// vY.getAsDouble(), -// headingHorizontal.getAsDouble(), -// headingVertical.getAsDouble()); - -// // Prevent Movement After Auto -// if (initRotation) { -// if (headingHorizontal.getAsDouble() == 0 && headingVertical.getAsDouble() == 0) { -// // Get the curretHeading -// Rotation2d firstLoopHeading = swerve.getHeading(); - -// // Set the Current Heading to the desired Heading -// desiredSpeeds = -// swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos()); -// } -// // Dont Init Rotation Again -// initRotation = false; -// } - -// // Limit velocity to prevent tippy -// Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); -// translation = -// SwerveMath.limitVelocity( -// translation, -// swerve.getFieldVelocity(), -// swerve.getPose(), -// PhysicalConstants.kLoopTime, -// PhysicalConstants.kRobotMass, -// List.of(PhysicalConstants.kChassis), -// swerve.getSwerveDriveConfiguration()); -// SmartDashboard.putNumber("LimitedTranslation", translation.getX()); -// SmartDashboard.putString("Translation", translation.toString()); - -// // Make the robot move -// swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); -// } - -// // Called once the command ends or is interrupted. -// @Override -// public void end(boolean interrupted) {} - -// // Returns true when the command should end. -// @Override -// public boolean isFinished() { -// return false; -// } -// } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java deleted file mode 100644 index d959948f..00000000 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java +++ /dev/null @@ -1,171 +0,0 @@ -// // Copyright (c) 2024 Az-FIRST -// // http://github.com/AZ-First -// // -// // This program is free software; you can redistribute it and/or -// // modify it under the terms of the GNU General Public License -// // version 3 as published by the Free Software Foundation or -// // available in the root directory of this project. -// // -// // 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. -// // -// // Copyright (c) FIRST and other WPILib contributors. -// // Open Source Software; you can modify and/or share it under the terms of -// // the WPILib BSD license file in the root directory of this project. -// // -// // NOTE: This module from the YAGSL Example Project - -// package frc.robot.commands.swervedrive.drivebase; - -// import edu.wpi.first.math.geometry.Rotation2d; -// import edu.wpi.first.math.geometry.Translation2d; -// import edu.wpi.first.math.kinematics.ChassisSpeeds; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.Constants.OperatorConstants; -// import frc.robot.Constants.PhysicalConstants; -// import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; -// import java.util.List; -// import java.util.function.BooleanSupplier; -// import java.util.function.DoubleSupplier; -// import swervelib.SwerveController; -// import swervelib.math.SwerveMath; - -// /** A more advanced Swerve Control System that has 4 buttons for which direction to face */ -// public class YAGSL_AbsoluteDriveAdv extends Command { - -// private final YAGSLSwerve swerve; -// private final DoubleSupplier vX, vY; -// private final DoubleSupplier headingAdjust; -// private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; -// private boolean resetHeading = false; - -// /** -// * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation -// inputs, -// * where x is torwards/away from alliance wall and y is left/right. Heading Adjust changes the -// * current heading after being multipied by a constant. The look booleans are shortcuts to get -// the -// * robot to face a certian direction. Based off of ideas in -// * https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172 -// * -// * @param swerve The swerve drivebase subsystem. -// * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the -// range -// * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. -// * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the -// range -// * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when -// * looking through the driver station glass. -// * @param headingAdjust DoubleSupplier that supplies the component of the robot's heading angle -// * that should be adjusted. Should range from -1 to 1 with deadband already accounted for. -// * @param lookAway Face the robot towards the opposing alliance's wall in the same direction -// the -// * driver is facing -// * @param lookTowards Face the robot towards the driver -// * @param lookLeft Face the robot left -// * @param lookRight Face the robot right -// */ -// public YAGSL_AbsoluteDriveAdv( -// YAGSLSwerve swerve, -// DoubleSupplier vX, -// DoubleSupplier vY, -// DoubleSupplier headingAdjust, -// BooleanSupplier lookAway, -// BooleanSupplier lookTowards, -// BooleanSupplier lookLeft, -// BooleanSupplier lookRight) { -// this.swerve = swerve; -// this.vX = vX; -// this.vY = vY; -// this.headingAdjust = headingAdjust; -// this.lookAway = lookAway; -// this.lookTowards = lookTowards; -// this.lookLeft = lookLeft; -// this.lookRight = lookRight; - -// addRequirements(swerve); -// } - -// @Override -// public void initialize() { -// resetHeading = true; -// } - -// // Called every time the scheduler runs while the command is scheduled. -// @Override -// public void execute() { -// double headingX = 0; -// double headingY = 0; - -// // These are written to allow combinations for 45 angles -// // Face Away from Drivers -// if (lookAway.getAsBoolean()) { -// headingY = -1; -// } -// // Face Right -// if (lookRight.getAsBoolean()) { -// headingX = 1; -// } -// // Face Left -// if (lookLeft.getAsBoolean()) { -// headingX = -1; -// } -// // Face Towards the Drivers -// if (lookTowards.getAsBoolean()) { -// headingY = 1; -// } - -// // Prevent Movement After Auto -// if (resetHeading) { -// if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) == 0) { -// // Get the curret Heading -// Rotation2d currentHeading = swerve.getHeading(); - -// // Set the Current Heading to the desired Heading -// headingX = currentHeading.getSin(); -// headingY = currentHeading.getCos(); -// } -// // Dont reset Heading Again -// resetHeading = false; -// } - -// ChassisSpeeds desiredSpeeds = -// swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY); - -// // Limit velocity to prevent tippy -// Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); -// translation = -// SwerveMath.limitVelocity( -// translation, -// swerve.getFieldVelocity(), -// swerve.getPose(), -// PhysicalConstants.kLoopTime, -// PhysicalConstants.kRobotMass, -// List.of(PhysicalConstants.kChassis), -// swerve.getSwerveDriveConfiguration()); -// SmartDashboard.putNumber("LimitedTranslation", translation.getX()); -// SmartDashboard.putString("Translation", translation.toString()); - -// // Make the robot move -// if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { -// resetHeading = true; -// swerve.drive( -// translation, (OperatorConstants.kTurnConstant * -headingAdjust.getAsDouble()), true); -// } else { -// swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); -// } -// } - -// // Called once the command ends or is interrupted. -// @Override -// public void end(boolean interrupted) {} - -// // Returns true when the command should end. -// @Override -// public boolean isFinished() { -// return false; -// } -// } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteFieldDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteFieldDrive.java deleted file mode 100644 index 2fff696f..00000000 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteFieldDrive.java +++ /dev/null @@ -1,107 +0,0 @@ -// // Copyright (c) 2024 Az-FIRST -// // http://github.com/AZ-First -// // -// // This program is free software; you can redistribute it and/or -// // modify it under the terms of the GNU General Public License -// // version 3 as published by the Free Software Foundation or -// // available in the root directory of this project. -// // -// // 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. -// // -// // Copyright (c) FIRST and other WPILib contributors. -// // Open Source Software; you can modify and/or share it under the terms of -// // the WPILib BSD license file in the root directory of this project. -// // -// // NOTE: This module from the YAGSL Example Project - -// package frc.robot.commands.swervedrive.drivebase; - -// import edu.wpi.first.math.geometry.Rotation2d; -// import edu.wpi.first.math.geometry.Translation2d; -// import edu.wpi.first.math.kinematics.ChassisSpeeds; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.Constants.PhysicalConstants; -// import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; -// import java.util.List; -// import java.util.function.DoubleSupplier; -// import swervelib.SwerveController; -// import swervelib.math.SwerveMath; - -// /** An example command that uses an example subsystem. */ -// public class YAGSL_AbsoluteFieldDrive extends Command { - -// private final YAGSLSwerve swerve; -// private final DoubleSupplier vX, vY, heading; - -// /** -// * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation -// inputs, -// * where x is torwards/away from alliance wall and y is left/right. headingHorzontal and -// * headingVertical are the Cartesian coordinates from which the robot's angle will be derived— -// * they will be converted to a polar angle, which the robot will rotate to. -// * -// * @param swerve The swerve drivebase subsystem. -// * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the -// range -// * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. -// * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the -// range -// * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when -// * looking through the driver station glass. -// * @param heading DoubleSupplier that supplies the robot's heading angle. -// */ -// public YAGSL_AbsoluteFieldDrive( -// YAGSLSwerve swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier heading) { -// this.swerve = swerve; -// this.vX = vX; -// this.vY = vY; -// this.heading = heading; - -// addRequirements(swerve); -// } - -// @Override -// public void initialize() {} - -// // Called every time the scheduler runs while the command is scheduled. -// @Override -// public void execute() { - -// // Get the desired chassis speeds based on a 2 joystick module. - -// ChassisSpeeds desiredSpeeds = -// swerve.getTargetSpeeds( -// vX.getAsDouble(), vY.getAsDouble(), new Rotation2d(heading.getAsDouble() * Math.PI)); - -// // Limit velocity to prevent tippy -// Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); -// translation = -// SwerveMath.limitVelocity( -// translation, -// swerve.getFieldVelocity(), -// swerve.getPose(), -// PhysicalConstants.kLoopTime, -// PhysicalConstants.kRobotMass, -// List.of(PhysicalConstants.kChassis), -// swerve.getSwerveDriveConfiguration()); -// SmartDashboard.putNumber("LimitedTranslation", translation.getX()); -// SmartDashboard.putString("Translation", translation.toString()); - -// // Make the robot move -// swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); -// } - -// // Called once the command ends or is interrupted. -// @Override -// public void end(boolean interrupted) {} - -// // Returns true when the command should end. -// @Override -// public boolean isFinished() { -// return false; -// } -// } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index bb70d422..69d988c1 100755 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -11,7 +11,8 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.math.util.Units; -import frc.robot.subsystems.swervedrive_ignore.SwerveSubsystem; + +// import frc.robot.subsystems.swervedrive_ignore.SwerveSubsystem; // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html @@ -189,6 +190,6 @@ public class TunerConstants { kInvertRightSide) .withSteerMotorInverted(kBackRightSteerInvert); - public static final SwerveSubsystem DriveTrain = - new SwerveSubsystem(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); + // public static final SwerveSubsystem DriveTrain = + // new SwerveSubsystem(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index bdce538c..99ce6ab4 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -113,31 +113,38 @@ public Drive() { throw new RuntimeException("Invalid Swerve Drive Type"); } - // Configure AutoBuilder for PathPlanner - AutoBuilder.configureHolonomic( - this::getPose, - this::setPose, - () -> kinematics.toChassisSpeeds(getModuleStates()), - this::runVelocity, - new HolonomicPathFollowerConfig( - DrivebaseConstants.kMaxLinearSpeed, - Units.inchesToMeters(kDriveBaseRadius), - new ReplanningConfig()), - () -> - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red, - this); - Pathfinding.setPathfinder(new LocalADStarAK()); - PathPlannerLogging.setLogActivePathCallback( - (activePath) -> { - Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); - }); - PathPlannerLogging.setLogTargetPoseCallback( - (targetPose) -> { - Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); - }); - + // Configure Autonomous Path Building based on `AutoType` + switch (Constants.getAutoType()) { + case PATHPLANNER: + // Configure AutoBuilder for PathPlanner + AutoBuilder.configureHolonomic( + this::getPose, + this::setPose, + () -> kinematics.toChassisSpeeds(getModuleStates()), + this::runVelocity, + new HolonomicPathFollowerConfig( + DrivebaseConstants.kMaxLinearSpeed, + Units.inchesToMeters(kDriveBaseRadius), + new ReplanningConfig()), + () -> + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red, + this); + Pathfinding.setPathfinder(new LocalADStarAK()); + PathPlannerLogging.setLogActivePathCallback( + (activePath) -> { + Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + }); + PathPlannerLogging.setLogTargetPoseCallback( + (targetPose) -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); + break; + case CHOREO: + break; + default: + } // Configure SysId sysId = new SysIdRoutine( @@ -335,7 +342,7 @@ public static Translation2d[] getModuleTranslations() { }; } - public T getGyro() { + public Object getGyro() { return gyroIO.getGyro(); } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 6a4f2861..8b1b9f0c 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -18,7 +18,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; -public interface GyroIO { +public interface GyroIO { @AutoLog public static class GyroIOInputs { public boolean connected = false; @@ -30,5 +30,5 @@ public default void updateInputs(GyroIOInputs inputs) {} public default void zero() {} - public abstract T getGyro(); + public abstract T getGyro(); } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index 40b4709b..009846aa 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -23,7 +23,7 @@ import edu.wpi.first.wpilibj.SPI; /** IO implementation for Pigeon2 */ -public class GyroIONavX implements GyroIO { +public class GyroIONavX implements GyroIO { private final AHRS navx; // Constructor, taking default values @@ -39,7 +39,7 @@ public GyroIONavX() { } // Return the Pigeon2 instance - @SuppressWarnings("unchecked") + @Override public AHRS getGyro() { return navx; } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 28a27a4d..7ce51c18 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -28,7 +28,7 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; /** IO implementation for Pigeon2 */ -public class GyroIOPigeon2 implements GyroIO { +public class GyroIOPigeon2 implements GyroIO { private final Pigeon2 pigeon; private final StatusSignal yaw; private final StatusSignal yawVelocity; @@ -47,7 +47,7 @@ public GyroIOPigeon2() { } // Return the Pigeon2 instance - @SuppressWarnings("unchecked") + @Override public Pigeon2 getGyro() { return pigeon; } diff --git a/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java index bcb589d9..d2eb761c 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java @@ -1,374 +1,382 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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. -// -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. -// -// NOTE: This module based on the CTRE Phoenix6 examples -// https://github.com/CrossTheRoadElec/Phoenix6-Examples - -package frc.robot.subsystems.swervedrive_ignore; - -import static edu.wpi.first.units.Units.Volts; -import static frc.robot.Constants.DrivebaseConstants.*; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; -import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; -import com.pathplanner.lib.util.ReplanningConfig; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants.DrivebaseConstants; -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.vision.Vision; -import java.util.function.Supplier; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -/** - * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used - * in command-based projects easily. - */ -public class SwerveSubsystem extends SwerveDrivetrain implements Subsystem { - /** CTRE constants */ - private static final double kSimLoopPeriod = 0.005; // 5 ms - - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - /** PhotonVision class to keep an accurate odometry */ - private Vision vision; - - /** Enable vision odometry updates while driving */ - private final boolean visionDriveTest = false; - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private final Rotation2d RedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean hasAppliedOperatorPerspective = false; - - private final SwerveRequest.ApplyChassisSpeeds AutoRequest = - new SwerveRequest.ApplyChassisSpeeds(); - - private final SwerveRequest.SysIdSwerveTranslation TranslationCharacterization = - new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveRotation RotationCharacterization = - new SwerveRequest.SysIdSwerveRotation(); - private final SwerveRequest.SysIdSwerveSteerGains SteerCharacterization = - new SwerveRequest.SysIdSwerveSteerGains(); - - /* Use one of these sysidroutines for your particular test */ - private SysIdRoutine SysIdRoutineTranslation = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - Volts.of(4), - null, - (state) -> SignalLogger.writeString("state", state.toString())), - new SysIdRoutine.Mechanism( - (volts) -> setControl(TranslationCharacterization.withVolts(volts)), null, this)); - - private final SysIdRoutine SysIdRoutineRotation = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - Volts.of(4), - null, - (state) -> SignalLogger.writeString("state", state.toString())), - new SysIdRoutine.Mechanism( - (volts) -> setControl(RotationCharacterization.withVolts(volts)), null, this)); - private final SysIdRoutine SysIdRoutineSteer = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - Volts.of(7), - null, - (state) -> SignalLogger.writeString("state", state.toString())), - new SysIdRoutine.Mechanism( - (volts) -> setControl(SteerCharacterization.withVolts(volts)), null, this)); - - /* Change this to the sysid routine you want to test */ - private final SysIdRoutine RoutineToApply = SysIdRoutineTranslation; - - public SwerveSubsystem( - SwerveDrivetrainConstants driveTrainConstants, - double OdometryUpdateFrequency, - SwerveModuleConstants... modules) { - super(driveTrainConstants, OdometryUpdateFrequency, modules); - configurePathPlanner(); - if (Utils.isSimulation()) { - startSimThread(); - } - Logger.recordOutput("SwerveDrive/Comment1", "This is a comment!"); - } - - public SwerveSubsystem( - SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { - super(driveTrainConstants, modules); - configurePathPlanner(); - if (Utils.isSimulation()) { - startSimThread(); - } - Logger.recordOutput("SwerveDrive/Comment2", "This is a comment!"); - } - - private void configurePathPlanner() { - double driveBaseRadius = 0; - for (var moduleLocation : m_moduleLocations) { - driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm()); - } - - AutoBuilder.configureHolonomic( - () -> this.getState().Pose, // Supplier of current robot pose - this::seedFieldRelative, // Consumer for seeding pose against auto - this::getCurrentRobotChassisSpeeds, - (speeds) -> - this.setControl( - AutoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot - new HolonomicPathFollowerConfig( - new PIDConstants(10, 0, 0), - new PIDConstants(10, 0, 0), - TunerConstants.kSpeedAt12VoltsMps, - driveBaseRadius, - new ReplanningConfig()), - () -> - DriverStation.getAlliance().orElse(Alliance.Blue) - == Alliance - .Red, // Assume the path needs to be flipped for Red vs Blue, this is normally - // the case - this); // Subsystem for requirements - } - - /* Section for defining TeleOp Commands */ - // Field-centric driving in open loop mode - public final SwerveRequest.FieldCentric drive = - new SwerveRequest.FieldCentric() - .withDeadband(DrivebaseConstants.kMaxLinearSpeed * 0.1) - .withRotationalDeadband(DrivebaseConstants.kMaxAngularSpeed * 0.1) // Add a 10% deadband - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - - public final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - public final SwerveRequest.RobotCentric forwardStraight = - new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); - public final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } - - public Command getAutoPath(String pathName) { - return new PathPlannerAuto(pathName); - } - - /* - * Both the sysid commands are specific to one particular sysid routine, change - * which one you're trying to characterize - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return RoutineToApply.quasistatic(direction); - } - - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return RoutineToApply.dynamic(direction); - } - - public ChassisSpeeds getCurrentRobotChassisSpeeds() { - return m_kinematics.toChassisSpeeds(getState().ModuleStates); - } - - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); - - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = - new Notifier( - () -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; - - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); - } - - /** Periodic function -- update odometry and log everything */ - @Override - public void periodic() { - /* Periodically try to apply the operator perspective */ - /* If we haven't applied the operator perspective before, then we should apply it regardless of DS state */ - /* This allows us to correct the perspective in case the robot code restarts mid-match */ - /* Otherwise, only check and apply the operator perspective if the DS is disabled */ - /* This ensures driving behavior doesn't change until an explicit disable event occurs during testing*/ - if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance() - .ifPresent( - (allianceColor) -> { - this.setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? RedAlliancePerspectiveRotation - : BlueAlliancePerspectiveRotation); - hasAppliedOperatorPerspective = true; - }); - } - - // // When vision is enabled we must manually update odometry in SwerveDrive - // if (visionDriveTest) { - // vision.updatePoseEstimation(this); - // } - - /** Log Telemetry Data to AdvantageKit */ - Logger.recordOutput("SwerveDrive/Comment", "This is a comment!"); - // Logger.recordOutput("SwerveDrive/Telemetry/moduleCount", this.ModuleCount); - // Logger.recordOutput("SwerveDrive/Telemetry/wheelLocations", this.m_moduleLocations); - Logger.recordOutput("SwerveDrive/Telemetry/measuredStates", getState().ModuleStates); - // Logger.recordOutput("SwerveDive/Telemetry/desiredStates", - // SwerveDriveTelemetry.desiredStates); - // Logger.recordOutput("SwerveDive/Telemetry/robotRotation", - // SwerveDriveTelemetry.robotRotation); - // Logger.recordOutput("SwerveDive/Telemetry/maxSpeed", SwerveDriveTelemetry.maxSpeed); - // Logger.recordOutput("SwerveDive/Telemetry/rotationUnit", SwerveDriveTelemetry.rotationUnit); - // Logger.recordOutput("SwerveDive/Telemetry/sizeLeftRight", - // SwerveDriveTelemetry.sizeLeftRight); - // Logger.recordOutput("SwerveDive/Telemetry/sizeFrontBack", - // SwerveDriveTelemetry.sizeFrontBack); - // Logger.recordOutput( - // "SwerveDive/Telemetry/forwardDirection", SwerveDriveTelemetry.forwardDirection); - // Logger.recordOutput( - // "SwerveDive/Telemetry/maxAngularVelocity", SwerveDriveTelemetry.maxAngularVelocity); - // Logger.recordOutput( - // "SwerveDive/Telemetry/measuredChassisSpeeds", - // SwerveDriveTelemetry.measuredChassisSpeeds); - // Logger.recordOutput( - // "SwerveDive/Telemetry/desiredChassisSpeeds", SwerveDriveTelemetry.desiredChassisSpeeds); - - // /** Log Swerve Drive States to AdvantageKit */ - // getModuleStates(); - // getDesiredStates(); - // Logger.recordOutput( - // "SwerveDive/States/RobotRotation", - // SwerveDriveTelemetry.rotationUnit == "degrees" - // ? Rotation2d.fromDegrees(SwerveDriveTelemetry.robotRotation) - // : Rotation2d.fromRadians(SwerveDriveTelemetry.robotRotation)); - } - - /************************************************************************* */ - /* COMMAND SECTION -- Drivebase-only Commands */ - - /** - * Get the path follower with events. - * - * @param pathName PathPlanner path name. - * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. - */ - public Command getAutonomousCommand(String pathName) { - // Create a path following command using AutoBuilder. This will also trigger event markers. - return new PathPlannerAuto(pathName); - } - - /** - * Use PathPlanner Path finding to go to a point on the field. - * - * @param pose Target {@link Pose2d} to go to. - * @return PathFinding command - */ - public Command driveToPose(Pose2d pose) { - // Create the constraints to use while pathfinding - PathConstraints constraints = - new PathConstraints(kMaxLinearSpeed, kMaxLinearAccel, kMaxAngularSpeed, kMaxAngularAccel); - - // Since AutoBuilder is configured, we can use it to build pathfinding commands - return AutoBuilder.pathfindToPose( - pose, - constraints, - 0.0, // Goal end velocity in meters/sec - 0.0 // Rotation delay distance in meters. This is how far the robot should travel before - // attempting to rotate. - ); - } - - /************************************************************************* */ - /* UTILITY SECTION -- Utility methods */ - - /** - * Gets the current pose (position and rotation) of the robot, as reported by odometry. - * - * @return The robot's pose - */ - @AutoLogOutput(key = "Odometry/RobotPose") - public Pose2d getPose() { - return getState().Pose; - } - - /** - * Sets the drive motors to brake/coast mode. - * - * @param brake True to set motors to brake mode, false for coast. - */ - public void setMotorBrake(boolean brake) { - if (brake) { - this.configNeutralMode(NeutralModeValue.Brake); - } else { - this.configNeutralMode(NeutralModeValue.Coast); - } - } - - // /** Returns the module states (turn angles and drive velocities) for all of the modules. */ - // @AutoLogOutput(key = "SwerveDive/States/Measured") - // private SwerveModuleState[] getModuleStates() { - // SwerveModuleState[] states = new SwerveModuleState[4]; - // for (int i = 0; i < 4; i++) { - // states[i] = - // new SwerveModuleState( - // SwerveDriveTelemetry.measuredStates[(i * 2) + 1], - // Rotation2d.fromDegrees(SwerveDriveTelemetry.measuredStates[i * 2])); - // } - // return states; - // } - - // /** Returns the desired states (turn angles and drive velocities) for all of the modules. */ - // @AutoLogOutput(key = "SwerveDive/States/Desired") - // private SwerveModuleState[] getDesiredStates() { - // SwerveModuleState[] states = new SwerveModuleState[4]; - // for (int i = 0; i < 4; i++) { - // states[i] = - // new SwerveModuleState( - // SwerveDriveTelemetry.desiredStates[(i * 2) + 1], - // Rotation2d.fromDegrees(SwerveDriveTelemetry.desiredStates[i * 2])); - // } - // return states; - // } -} +// // Copyright (c) 2024 Az-FIRST +// // http://github.com/AZ-First +// // +// // This program is free software; you can redistribute it and/or +// // modify it under the terms of the GNU General Public License +// // version 3 as published by the Free Software Foundation or +// // available in the root directory of this project. +// // +// // 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. +// // +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. +// // +// // NOTE: This module based on the CTRE Phoenix6 examples +// // https://github.com/CrossTheRoadElec/Phoenix6-Examples + +// package frc.robot.subsystems.swervedrive_ignore; + +// import static edu.wpi.first.units.Units.Volts; +// import static frc.robot.Constants.DrivebaseConstants.*; + +// import com.ctre.phoenix6.SignalLogger; +// import com.ctre.phoenix6.Utils; +// import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; +// import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +// import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +// import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; +// import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +// import com.ctre.phoenix6.signals.NeutralModeValue; +// import com.pathplanner.lib.auto.AutoBuilder; +// import com.pathplanner.lib.commands.PathPlannerAuto; +// import com.pathplanner.lib.path.PathConstraints; +// import com.pathplanner.lib.path.PathPlannerPath; +// import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +// import com.pathplanner.lib.util.PIDConstants; +// import com.pathplanner.lib.util.ReplanningConfig; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.kinematics.ChassisSpeeds; +// import edu.wpi.first.wpilibj.DriverStation; +// import edu.wpi.first.wpilibj.DriverStation.Alliance; +// import edu.wpi.first.wpilibj.Notifier; +// import edu.wpi.first.wpilibj.RobotController; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.Subsystem; +// import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +// import frc.robot.Constants.DrivebaseConstants; +// import frc.robot.generated.TunerConstants; +// import frc.robot.subsystems.vision.Vision; +// import java.util.function.Supplier; +// import org.littletonrobotics.junction.AutoLogOutput; +// import org.littletonrobotics.junction.Logger; + +// /** +// * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be +// used +// * in command-based projects easily. +// */ +// public class SwerveSubsystem extends SwerveDrivetrain implements Subsystem { +// /** CTRE constants */ +// private static final double kSimLoopPeriod = 0.005; // 5 ms + +// private Notifier m_simNotifier = null; +// private double m_lastSimTime; + +// /** PhotonVision class to keep an accurate odometry */ +// private Vision vision; + +// /** Enable vision odometry updates while driving */ +// private final boolean visionDriveTest = false; + +// /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ +// private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); +// /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ +// private final Rotation2d RedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); +// /* Keep track if we've ever applied the operator perspective before or not */ +// private boolean hasAppliedOperatorPerspective = false; + +// private final SwerveRequest.ApplyChassisSpeeds AutoRequest = +// new SwerveRequest.ApplyChassisSpeeds(); + +// private final SwerveRequest.SysIdSwerveTranslation TranslationCharacterization = +// new SwerveRequest.SysIdSwerveTranslation(); +// private final SwerveRequest.SysIdSwerveRotation RotationCharacterization = +// new SwerveRequest.SysIdSwerveRotation(); +// private final SwerveRequest.SysIdSwerveSteerGains SteerCharacterization = +// new SwerveRequest.SysIdSwerveSteerGains(); + +// /* Use one of these sysidroutines for your particular test */ +// private SysIdRoutine SysIdRoutineTranslation = +// new SysIdRoutine( +// new SysIdRoutine.Config( +// null, +// Volts.of(4), +// null, +// (state) -> SignalLogger.writeString("state", state.toString())), +// new SysIdRoutine.Mechanism( +// (volts) -> setControl(TranslationCharacterization.withVolts(volts)), null, this)); + +// private final SysIdRoutine SysIdRoutineRotation = +// new SysIdRoutine( +// new SysIdRoutine.Config( +// null, +// Volts.of(4), +// null, +// (state) -> SignalLogger.writeString("state", state.toString())), +// new SysIdRoutine.Mechanism( +// (volts) -> setControl(RotationCharacterization.withVolts(volts)), null, this)); +// private final SysIdRoutine SysIdRoutineSteer = +// new SysIdRoutine( +// new SysIdRoutine.Config( +// null, +// Volts.of(7), +// null, +// (state) -> SignalLogger.writeString("state", state.toString())), +// new SysIdRoutine.Mechanism( +// (volts) -> setControl(SteerCharacterization.withVolts(volts)), null, this)); + +// /* Change this to the sysid routine you want to test */ +// private final SysIdRoutine RoutineToApply = SysIdRoutineTranslation; + +// public SwerveSubsystem( +// SwerveDrivetrainConstants driveTrainConstants, +// double OdometryUpdateFrequency, +// SwerveModuleConstants... modules) { +// super(driveTrainConstants, OdometryUpdateFrequency, modules); +// configurePathPlanner(); +// if (Utils.isSimulation()) { +// startSimThread(); +// } +// Logger.recordOutput("SwerveDrive/Comment1", "This is a comment!"); +// } + +// public SwerveSubsystem( +// SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { +// super(driveTrainConstants, modules); +// configurePathPlanner(); +// if (Utils.isSimulation()) { +// startSimThread(); +// } +// Logger.recordOutput("SwerveDrive/Comment2", "This is a comment!"); +// } + +// private void configurePathPlanner() { +// double driveBaseRadius = 0; +// for (var moduleLocation : m_moduleLocations) { +// driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm()); +// } + +// AutoBuilder.configureHolonomic( +// () -> this.getState().Pose, // Supplier of current robot pose +// this::seedFieldRelative, // Consumer for seeding pose against auto +// this::getCurrentRobotChassisSpeeds, +// (speeds) -> +// this.setControl( +// AutoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot +// new HolonomicPathFollowerConfig( +// new PIDConstants(10, 0, 0), +// new PIDConstants(10, 0, 0), +// TunerConstants.kSpeedAt12VoltsMps, +// driveBaseRadius, +// new ReplanningConfig()), +// () -> +// DriverStation.getAlliance().orElse(Alliance.Blue) +// == Alliance +// .Red, // Assume the path needs to be flipped for Red vs Blue, this is +// normally +// // the case +// this); // Subsystem for requirements +// } + +// /* Section for defining TeleOp Commands */ +// // Field-centric driving in open loop mode +// public final SwerveRequest.FieldCentric drive = +// new SwerveRequest.FieldCentric() +// .withDeadband(DrivebaseConstants.kMaxLinearSpeed * 0.1) +// .withRotationalDeadband(DrivebaseConstants.kMaxAngularSpeed * 0.1) // Add a 10% +// deadband +// .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + +// public final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); +// public final SwerveRequest.RobotCentric forwardStraight = +// new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); +// public final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + +// public Command applyRequest(Supplier requestSupplier) { +// return run(() -> this.setControl(requestSupplier.get())); +// } + +// public Command getAutoPath(String pathName) { +// return new PathPlannerAuto(pathName); +// } + +// /* +// * Both the sysid commands are specific to one particular sysid routine, change +// * which one you're trying to characterize +// */ +// public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { +// return RoutineToApply.quasistatic(direction); +// } + +// public Command sysIdDynamic(SysIdRoutine.Direction direction) { +// return RoutineToApply.dynamic(direction); +// } + +// public ChassisSpeeds getCurrentRobotChassisSpeeds() { +// return m_kinematics.toChassisSpeeds(getState().ModuleStates); +// } + +// private void startSimThread() { +// m_lastSimTime = Utils.getCurrentTimeSeconds(); + +// /* Run simulation at a faster rate so PID gains behave more reasonably */ +// m_simNotifier = +// new Notifier( +// () -> { +// final double currentTime = Utils.getCurrentTimeSeconds(); +// double deltaTime = currentTime - m_lastSimTime; +// m_lastSimTime = currentTime; + +// /* use the measured time delta, get battery voltage from WPILib */ +// updateSimState(deltaTime, RobotController.getBatteryVoltage()); +// }); +// m_simNotifier.startPeriodic(kSimLoopPeriod); +// } + +// /** Periodic function -- update odometry and log everything */ +// @Override +// public void periodic() { +// /* Periodically try to apply the operator perspective */ +// /* If we haven't applied the operator perspective before, then we should apply it regardless +// of DS state */ +// /* This allows us to correct the perspective in case the robot code restarts mid-match */ +// /* Otherwise, only check and apply the operator perspective if the DS is disabled */ +// /* This ensures driving behavior doesn't change until an explicit disable event occurs during +// testing*/ +// if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) { +// DriverStation.getAlliance() +// .ifPresent( +// (allianceColor) -> { +// this.setOperatorPerspectiveForward( +// allianceColor == Alliance.Red +// ? RedAlliancePerspectiveRotation +// : BlueAlliancePerspectiveRotation); +// hasAppliedOperatorPerspective = true; +// }); +// } + +// // // When vision is enabled we must manually update odometry in SwerveDrive +// // if (visionDriveTest) { +// // vision.updatePoseEstimation(this); +// // } + +// /** Log Telemetry Data to AdvantageKit */ +// Logger.recordOutput("SwerveDrive/Comment", "This is a comment!"); +// // Logger.recordOutput("SwerveDrive/Telemetry/moduleCount", this.ModuleCount); +// // Logger.recordOutput("SwerveDrive/Telemetry/wheelLocations", this.m_moduleLocations); +// Logger.recordOutput("SwerveDrive/Telemetry/measuredStates", getState().ModuleStates); +// // Logger.recordOutput("SwerveDive/Telemetry/desiredStates", +// // SwerveDriveTelemetry.desiredStates); +// // Logger.recordOutput("SwerveDive/Telemetry/robotRotation", +// // SwerveDriveTelemetry.robotRotation); +// // Logger.recordOutput("SwerveDive/Telemetry/maxSpeed", SwerveDriveTelemetry.maxSpeed); +// // Logger.recordOutput("SwerveDive/Telemetry/rotationUnit", +// SwerveDriveTelemetry.rotationUnit); +// // Logger.recordOutput("SwerveDive/Telemetry/sizeLeftRight", +// // SwerveDriveTelemetry.sizeLeftRight); +// // Logger.recordOutput("SwerveDive/Telemetry/sizeFrontBack", +// // SwerveDriveTelemetry.sizeFrontBack); +// // Logger.recordOutput( +// // "SwerveDive/Telemetry/forwardDirection", SwerveDriveTelemetry.forwardDirection); +// // Logger.recordOutput( +// // "SwerveDive/Telemetry/maxAngularVelocity", SwerveDriveTelemetry.maxAngularVelocity); +// // Logger.recordOutput( +// // "SwerveDive/Telemetry/measuredChassisSpeeds", +// // SwerveDriveTelemetry.measuredChassisSpeeds); +// // Logger.recordOutput( +// // "SwerveDive/Telemetry/desiredChassisSpeeds", +// SwerveDriveTelemetry.desiredChassisSpeeds); + +// // /** Log Swerve Drive States to AdvantageKit */ +// // getModuleStates(); +// // getDesiredStates(); +// // Logger.recordOutput( +// // "SwerveDive/States/RobotRotation", +// // SwerveDriveTelemetry.rotationUnit == "degrees" +// // ? Rotation2d.fromDegrees(SwerveDriveTelemetry.robotRotation) +// // : Rotation2d.fromRadians(SwerveDriveTelemetry.robotRotation)); +// } + +// /************************************************************************* */ +// /* COMMAND SECTION -- Drivebase-only Commands */ + +// /** +// * Get the path follower with events. +// * +// * @param pathName PathPlanner path name. +// * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. +// */ +// public Command getAutonomousCommand(String pathName) { +// // Create a path following command using AutoBuilder. This will also trigger event markers. +// return new PathPlannerAuto(pathName); +// } + +// /** +// * Use PathPlanner Path finding to go to a point on the field. +// * +// * @param pose Target {@link Pose2d} to go to. +// * @return PathFinding command +// */ +// public Command driveToPose(Pose2d pose) { +// // Create the constraints to use while pathfinding +// PathConstraints constraints = +// new PathConstraints(kMaxLinearSpeed, kMaxLinearAccel, kMaxAngularSpeed, +// kMaxAngularAccel); + +// // Since AutoBuilder is configured, we can use it to build pathfinding commands +// return AutoBuilder.pathfindToPose( +// pose, +// constraints, +// 0.0, // Goal end velocity in meters/sec +// 0.0 // Rotation delay distance in meters. This is how far the robot should travel before +// // attempting to rotate. +// ); +// } + +// /************************************************************************* */ +// /* UTILITY SECTION -- Utility methods */ + +// /** +// * Gets the current pose (position and rotation) of the robot, as reported by odometry. +// * +// * @return The robot's pose +// */ +// @AutoLogOutput(key = "Odometry/RobotPose") +// public Pose2d getPose() { +// return getState().Pose; +// } + +// /** +// * Sets the drive motors to brake/coast mode. +// * +// * @param brake True to set motors to brake mode, false for coast. +// */ +// public void setMotorBrake(boolean brake) { +// if (brake) { +// this.configNeutralMode(NeutralModeValue.Brake); +// } else { +// this.configNeutralMode(NeutralModeValue.Coast); +// } +// } + +// // /** Returns the module states (turn angles and drive velocities) for all of the modules. */ +// // @AutoLogOutput(key = "SwerveDive/States/Measured") +// // private SwerveModuleState[] getModuleStates() { +// // SwerveModuleState[] states = new SwerveModuleState[4]; +// // for (int i = 0; i < 4; i++) { +// // states[i] = +// // new SwerveModuleState( +// // SwerveDriveTelemetry.measuredStates[(i * 2) + 1], +// // Rotation2d.fromDegrees(SwerveDriveTelemetry.measuredStates[i * 2])); +// // } +// // return states; +// // } + +// // /** Returns the desired states (turn angles and drive velocities) for all of the modules. */ +// // @AutoLogOutput(key = "SwerveDive/States/Desired") +// // private SwerveModuleState[] getDesiredStates() { +// // SwerveModuleState[] states = new SwerveModuleState[4]; +// // for (int i = 0; i < 4; i++) { +// // states[i] = +// // new SwerveModuleState( +// // SwerveDriveTelemetry.desiredStates[(i * 2) + 1], +// // Rotation2d.fromDegrees(SwerveDriveTelemetry.desiredStates[i * 2])); +// // } +// // return states; +// // } +// } diff --git a/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java b/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java index 817a3f0d..e697634e 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java +++ b/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java @@ -1,144 +1,149 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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. -// -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. -// -// NOTE: This module based on the CTRE Phoenix6 examples -// https://github.com/CrossTheRoadElec/Phoenix6-Examples - -package frc.robot.subsystems.swervedrive_ignore; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.networktables.DoubleArrayPublisher; -import edu.wpi.first.networktables.DoublePublisher; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StringPublisher; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; - -public class SwerveTelemetry { - private final double MaxSpeed; - - /** - * Construct a telemetry object, with the specified max speed of the robot - * - * @param maxSpeed Maximum speed in meters per second - */ - public SwerveTelemetry(double maxSpeed) { - MaxSpeed = maxSpeed; - SignalLogger.start(); - } - - /* What to publish over networktables for telemetry */ - private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); - - /* Robot pose for field positioning */ - private final NetworkTable table = inst.getTable("Pose"); - private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); - private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); - - /* Robot speeds for general checking */ - private final NetworkTable driveStats = inst.getTable("Drive"); - private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish(); - private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish(); - private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish(); - private final DoublePublisher odomFreq = - driveStats.getDoubleTopic("Odometry Frequency").publish(); - - /* Keep a reference of the last pose to calculate the speeds */ - private Pose2d m_lastPose = new Pose2d(); - private double lastTime = Utils.getCurrentTimeSeconds(); - - /* Mechanisms to represent the swerve module states */ - private final Mechanism2d[] m_moduleMechanisms = - new Mechanism2d[] { - new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), - }; - /* A direction and length changing ligament for speed representation */ - private final MechanismLigament2d[] m_moduleSpeeds = - new MechanismLigament2d[] { - m_moduleMechanisms[0] - .getRoot("RootSpeed", 0.5, 0.5) - .append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[1] - .getRoot("RootSpeed", 0.5, 0.5) - .append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[2] - .getRoot("RootSpeed", 0.5, 0.5) - .append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[3] - .getRoot("RootSpeed", 0.5, 0.5) - .append(new MechanismLigament2d("Speed", 0.5, 0)), - }; - /* A direction changing and length constant ligament for module direction */ - private final MechanismLigament2d[] m_moduleDirections = - new MechanismLigament2d[] { - m_moduleMechanisms[0] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[1] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[2] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[3] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - }; - - /* Accept the swerve drive state and telemeterize it to smartdashboard */ - public void telemeterize(SwerveDriveState state) { - /* Telemeterize the pose */ - Pose2d pose = state.Pose; - fieldTypePub.set("Field2d"); - fieldPub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); - - /* Telemeterize the robot's general speeds */ - double currentTime = Utils.getCurrentTimeSeconds(); - double diffTime = currentTime - lastTime; - lastTime = currentTime; - Translation2d distanceDiff = pose.minus(m_lastPose).getTranslation(); - m_lastPose = pose; - - Translation2d velocities = distanceDiff.div(diffTime); - - speed.set(velocities.getNorm()); - velocityX.set(velocities.getX()); - velocityY.set(velocities.getY()); - odomFreq.set(1.0 / state.OdometryPeriod); - - /* Telemeterize the module's states */ - for (int i = 0; i < 4; ++i) { - m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); - m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); - m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); - - SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); - } - - SignalLogger.writeDoubleArray( - "odometry", new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); - SignalLogger.writeDouble("odom period", state.OdometryPeriod, "seconds"); - } -} +// // Copyright (c) 2024 Az-FIRST +// // http://github.com/AZ-First +// // +// // This program is free software; you can redistribute it and/or +// // modify it under the terms of the GNU General Public License +// // version 3 as published by the Free Software Foundation or +// // available in the root directory of this project. +// // +// // 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. +// // +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. +// // +// // NOTE: This module based on the CTRE Phoenix6 examples +// // https://github.com/CrossTheRoadElec/Phoenix6-Examples + +// package frc.robot.subsystems.swervedrive_ignore; + +// import com.ctre.phoenix6.SignalLogger; +// import com.ctre.phoenix6.Utils; +// import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.networktables.DoubleArrayPublisher; +// import edu.wpi.first.networktables.DoublePublisher; +// import edu.wpi.first.networktables.NetworkTable; +// import edu.wpi.first.networktables.NetworkTableInstance; +// import edu.wpi.first.networktables.StringPublisher; +// import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +// import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj.util.Color; +// import edu.wpi.first.wpilibj.util.Color8Bit; + +// public class SwerveTelemetry { +// private final double MaxSpeed; + +// /** +// * Construct a telemetry object, with the specified max speed of the robot +// * +// * @param maxSpeed Maximum speed in meters per second +// */ +// public SwerveTelemetry(double maxSpeed) { +// MaxSpeed = maxSpeed; +// SignalLogger.start(); +// } + +// /* What to publish over networktables for telemetry */ +// private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + +// /* Robot pose for field positioning */ +// private final NetworkTable table = inst.getTable("Pose"); +// private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); +// private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + +// /* Robot speeds for general checking */ +// private final NetworkTable driveStats = inst.getTable("Drive"); +// private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish(); +// private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish(); +// private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish(); +// private final DoublePublisher odomFreq = +// driveStats.getDoubleTopic("Odometry Frequency").publish(); + +// /* Keep a reference of the last pose to calculate the speeds */ +// private Pose2d m_lastPose = new Pose2d(); +// private double lastTime = Utils.getCurrentTimeSeconds(); + +// /* Mechanisms to represent the swerve module states */ +// private final Mechanism2d[] m_moduleMechanisms = +// new Mechanism2d[] { +// new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, +// 1), +// }; +// /* A direction and length changing ligament for speed representation */ +// private final MechanismLigament2d[] m_moduleSpeeds = +// new MechanismLigament2d[] { +// m_moduleMechanisms[0] +// .getRoot("RootSpeed", 0.5, 0.5) +// .append(new MechanismLigament2d("Speed", 0.5, 0)), +// m_moduleMechanisms[1] +// .getRoot("RootSpeed", 0.5, 0.5) +// .append(new MechanismLigament2d("Speed", 0.5, 0)), +// m_moduleMechanisms[2] +// .getRoot("RootSpeed", 0.5, 0.5) +// .append(new MechanismLigament2d("Speed", 0.5, 0)), +// m_moduleMechanisms[3] +// .getRoot("RootSpeed", 0.5, 0.5) +// .append(new MechanismLigament2d("Speed", 0.5, 0)), +// }; +// /* A direction changing and length constant ligament for module direction */ +// private final MechanismLigament2d[] m_moduleDirections = +// new MechanismLigament2d[] { +// m_moduleMechanisms[0] +// .getRoot("RootDirection", 0.5, 0.5) +// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new +// Color8Bit(Color.kWhite))), +// m_moduleMechanisms[1] +// .getRoot("RootDirection", 0.5, 0.5) +// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new +// Color8Bit(Color.kWhite))), +// m_moduleMechanisms[2] +// .getRoot("RootDirection", 0.5, 0.5) +// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new +// Color8Bit(Color.kWhite))), +// m_moduleMechanisms[3] +// .getRoot("RootDirection", 0.5, 0.5) +// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new +// Color8Bit(Color.kWhite))), +// }; + +// /* Accept the swerve drive state and telemeterize it to smartdashboard */ +// public void telemeterize(SwerveDriveState state) { +// /* Telemeterize the pose */ +// Pose2d pose = state.Pose; +// fieldTypePub.set("Field2d"); +// fieldPub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); + +// /* Telemeterize the robot's general speeds */ +// double currentTime = Utils.getCurrentTimeSeconds(); +// double diffTime = currentTime - lastTime; +// lastTime = currentTime; +// Translation2d distanceDiff = pose.minus(m_lastPose).getTranslation(); +// m_lastPose = pose; + +// Translation2d velocities = distanceDiff.div(diffTime); + +// speed.set(velocities.getNorm()); +// velocityX.set(velocities.getX()); +// velocityY.set(velocities.getY()); +// odomFreq.set(1.0 / state.OdometryPeriod); + +// /* Telemeterize the module's states */ +// for (int i = 0; i < 4; ++i) { +// m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); +// m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); +// m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + +// SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); +// } + +// SignalLogger.writeDoubleArray( +// "odometry", new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); +// SignalLogger.writeDouble("odom period", state.OdometryPeriod, "seconds"); +// } +// } From de90c220618c940edd7d3d03acf91eaad76baa85 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sun, 1 Dec 2024 12:38:09 -0700 Subject: [PATCH 55/57] Updated TunerConstants file modified: src/main/java/frc/robot/generated/TunerConstants.java --- .../frc/robot/generated/TunerConstants.java | 25 +++++++++---------- 1 file changed, 12 insertions(+), 13 deletions(-) mode change 100755 => 100644 src/main/java/frc/robot/generated/TunerConstants.java diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java old mode 100755 new mode 100644 index 69d988c1..2efc84e8 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -12,7 +12,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.math.util.Units; -// import frc.robot.subsystems.swervedrive_ignore.SwerveSubsystem; +// import frc.robot.subsystems.CommandSwerveDrivetrain; // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html @@ -30,12 +30,10 @@ public class TunerConstants { // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors - public static final ClosedLoopOutputType steerClosedLoopOutput = - ClosedLoopOutputType.TorqueCurrentFOC; + public static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.Voltage; // The closed-loop output type to use for the drive motors; // This affects the PID/FF gains for the drive motors - public static final ClosedLoopOutputType driveClosedLoopOutput = - ClosedLoopOutputType.TorqueCurrentFOC; + public static final ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot @@ -59,13 +57,13 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12v applied output; // This needs to be tuned to your individual robot - public static final double kSpeedAt12VoltsMps = 5.21; + public static final double kSpeedAt12VoltsMps = 4.73; // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot public static final double kCoupleRatio = 3.5714285714285716; - public static final double kDriveGearRatio = 6.122448979591837; + public static final double kDriveGearRatio = 6.746031746031747; public static final double kSteerGearRatio = 21.428571428571427; public static final double kWheelRadiusInches = 2; @@ -113,7 +111,7 @@ public class TunerConstants { public static final int kFrontLeftDriveMotorId = 1; public static final int kFrontLeftSteerMotorId = 2; public static final int kFrontLeftEncoderId = 3; - public static final double kFrontLeftEncoderOffset = -0.303466796875; + public static final double kFrontLeftEncoderOffset = -0.4736328125; public static final boolean kFrontLeftSteerInvert = true; public static final double kFrontLeftXPosInches = 10.375; @@ -123,7 +121,7 @@ public class TunerConstants { public static final int kFrontRightDriveMotorId = 4; public static final int kFrontRightSteerMotorId = 5; public static final int kFrontRightEncoderId = 6; - public static final double kFrontRightEncoderOffset = -0.122802734375; + public static final double kFrontRightEncoderOffset = -0.1123046875; public static final boolean kFrontRightSteerInvert = true; public static final double kFrontRightXPosInches = 10.375; @@ -133,7 +131,7 @@ public class TunerConstants { public static final int kBackLeftDriveMotorId = 7; public static final int kBackLeftSteerMotorId = 8; public static final int kBackLeftEncoderId = 9; - public static final double kBackLeftEncoderOffset = 0.4814453125; + public static final double kBackLeftEncoderOffset = 0.4091796875; public static final boolean kBackLeftSteerInvert = true; public static final double kBackLeftXPosInches = -10.375; @@ -143,7 +141,7 @@ public class TunerConstants { public static final int kBackRightDriveMotorId = 10; public static final int kBackRightSteerMotorId = 11; public static final int kBackRightEncoderId = 12; - public static final double kBackRightEncoderOffset = 0.300537109375; + public static final double kBackRightEncoderOffset = 0.08544921875; public static final boolean kBackRightSteerInvert = true; public static final double kBackRightXPosInches = -10.375; @@ -190,6 +188,7 @@ public class TunerConstants { kInvertRightSide) .withSteerMotorInverted(kBackRightSteerInvert); - // public static final SwerveSubsystem DriveTrain = - // new SwerveSubsystem(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); + // public static final CommandSwerveDrivetrain DriveTrain = new + // CommandSwerveDrivetrain(DrivetrainConstants, FrontLeft, + // FrontRight, BackLeft, BackRight); } From 400068cbfc9f93ee3202677e3689f4fc58bb47ea Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Tue, 3 Dec 2024 12:47:25 -0700 Subject: [PATCH 56/57] Functioning drive base! Going to tag this as the 1.0.0-alpha-1 release modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/RobotContainer.java modified: src/main/java/frc/robot/generated/TunerConstants.java modified: src/main/java/frc/robot/subsystems/drive/DriveConstants.java modified: src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java modified: src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java --- src/main/java/frc/robot/Constants.java | 13 +++++ src/main/java/frc/robot/RobotContainer.java | 55 +++++++++++-------- .../frc/robot/generated/TunerConstants.java | 6 -- .../subsystems/drive/DriveConstants.java | 32 ++++++----- .../robot/subsystems/drive/GyroIOPigeon2.java | 2 +- .../subsystems/drive/ModuleIOTalonFX.java | 6 +- 6 files changed, 68 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bf06caaa..b5a72596 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -54,8 +54,10 @@ public final class Constants { */ private static RobotType robotType = RobotType.COMPBOT; + // Define swerve, auto, and vision types being used private static SwerveType swerveType = SwerveType.PHOENIX6; private static AutoType autoType = AutoType.PATHPLANNER; + private static VisionType visionType = VisionType.NONE; public static boolean disableHAL = false; @@ -126,6 +128,17 @@ public static AutoType getAutoType() { return autoType; } + /** Enumerate the supported vision types */ + public static enum VisionType { + PHOTON, // PhotonVision (https://docs.photonvision.org/en/latest/) + NONE // No cameras + } + + /** Get the current autonomous path planning type */ + public static VisionType getVisionType() { + return visionType; + } + /***************************************************************************/ /* The remainder of this file contains physical and/or software constants for the various subsystems of the robot */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1fbaebac..71767867 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,6 +39,7 @@ import frc.robot.Constants.AprilTagConstants; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.PowerConstants; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.accelerometer.Accelerometer; import frc.robot.subsystems.drive.Drive; @@ -105,9 +106,14 @@ public RobotContainer() { m_drivebase = new Drive(); m_flywheel = new Flywheel(new FlywheelIOSim()); // new Flywheel(new FlywheelIOTalonFX()); m_vision = - new Vision( - this::getAprilTagLayoutType, - new VisionIOPhoton(this::getAprilTagLayoutType, "Photon_CAMNAME")); + switch (Constants.getVisionType()) { + case PHOTON -> + new Vision( + this::getAprilTagLayoutType, + new VisionIOPhoton(this::getAprilTagLayoutType, "Photon_CAMNAME")); + case NONE -> new Vision(this::getAprilTagLayoutType); + default -> null; + }; m_accel = new Accelerometer(m_drivebase.getGyro()); break; @@ -131,7 +137,7 @@ public RobotContainer() { // ``PowerMonitoring`` takes all the non-drivebase subsystems for which // you wish to have power monitoring; DO NOT include ``m_drivebase``, // as that is automatically monitored. - m_power = new PowerMonitoring(m_flywheel); + m_power = null; // new PowerMonitoring(m_flywheel); // Set up the SmartDashboard Auto Chooser autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -192,9 +198,9 @@ private void configureBindings() { m_drivebase.setDefaultCommand( DriveCommands.fieldRelativeDrive( m_drivebase, - () -> -driverXbox.getRightY(), - () -> -driverXbox.getRightX(), - () -> -driverXbox.getLeftX())); + () -> -driverXbox.getLeftY(), + () -> -driverXbox.getLeftX(), + () -> driverXbox.getRightX())); // Example Commands // Press B button while driving --> ROBOT-CENTRIC @@ -205,9 +211,9 @@ private void configureBindings() { () -> DriveCommands.robotRelativeDrive( m_drivebase, - () -> -driverXbox.getRightY(), - () -> -driverXbox.getRightX(), - () -> -driverXbox.getLeftX()), + () -> -driverXbox.getLeftY(), + () -> -driverXbox.getLeftX(), + () -> driverXbox.getRightX()), m_drivebase)); // Press A button -> BRAKE @@ -327,32 +333,37 @@ public static class Ports { // // 0 1 // 2 3 - public static final RobotDeviceId FL_DRIVE = new RobotDeviceId(1, "DriveTrain", 1); - public static final RobotDeviceId FL_ROTATION = new RobotDeviceId(2, "DriveTrain", 2); + public static final RobotDeviceId FL_DRIVE = new RobotDeviceId(1, "DriveTrain", 18); + public static final RobotDeviceId FL_ROTATION = new RobotDeviceId(2, "DriveTrain", 19); public static final RobotDeviceId FL_CANCODER = new RobotDeviceId(3, "DriveTrain", null); - public static final RobotDeviceId FR_DRIVE = new RobotDeviceId(4, "DriveTrain", 3); - public static final RobotDeviceId FR_ROTATION = new RobotDeviceId(5, "DriveTrain", 4); + public static final RobotDeviceId FR_DRIVE = new RobotDeviceId(4, "DriveTrain", 17); + public static final RobotDeviceId FR_ROTATION = new RobotDeviceId(5, "DriveTrain", 16); public static final RobotDeviceId FR_CANCODER = new RobotDeviceId(6, "DriveTrain", null); - public static final RobotDeviceId BL_DRIVE = new RobotDeviceId(7, "DriveTrain", 5); - public static final RobotDeviceId BL_ROTATION = new RobotDeviceId(8, "DriveTrain", 6); + public static final RobotDeviceId BL_DRIVE = new RobotDeviceId(7, "DriveTrain", 1); + public static final RobotDeviceId BL_ROTATION = new RobotDeviceId(8, "DriveTrain", 0); public static final RobotDeviceId BL_CANCODER = new RobotDeviceId(9, "DriveTrain", null); - public static final RobotDeviceId BR_DRIVE = new RobotDeviceId(10, "DriveTrain", 7); - public static final RobotDeviceId BR_ROTATION = new RobotDeviceId(11, "DriveTrain", 8); + public static final RobotDeviceId BR_DRIVE = new RobotDeviceId(10, "DriveTrain", 2); + public static final RobotDeviceId BR_ROTATION = new RobotDeviceId(11, "DriveTrain", 3); public static final RobotDeviceId BR_CANCODER = new RobotDeviceId(12, "DriveTrain", null); public static final RobotDeviceId PIGEON = new RobotDeviceId(13, "DriveTrain", null); - /* POWER DISTRIBUTION CAN ID */ - public static final RobotDeviceId POWER_CAN_DEVICE_ID = new RobotDeviceId(1, null); + /* POWER DISTRIBUTION CAN ID (set by device type in PowerConstants) */ + public static final RobotDeviceId POWER_CAN_DEVICE_ID = + switch (PowerConstants.kPowerModule) { + case kRev -> new RobotDeviceId(1, null); + case kCTRE -> new RobotDeviceId(0, null); + default -> null; + }; /* SUBSYSTEM CAN DEVICE IDS */ // This is where mechanism subsystem devices are defined // Example: - public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, 9); - public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, 10); + public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, 8); + public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, 9); /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */ // This is where digital I/O feedback devices are defined diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 2efc84e8..f88d9bf1 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -12,8 +12,6 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.math.util.Units; -// import frc.robot.subsystems.CommandSwerveDrivetrain; - // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { @@ -187,8 +185,4 @@ public class TunerConstants { Units.inchesToMeters(kBackRightYPosInches), kInvertRightSide) .withSteerMotorInverted(kBackRightSteerInvert); - - // public static final CommandSwerveDrivetrain DriveTrain = new - // CommandSwerveDrivetrain(DrivetrainConstants, FrontLeft, - // FrontRight, BackLeft, BackRight); } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index da3fce00..d7f9463e 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -123,8 +123,8 @@ public class DriveConstants { kPigeonId = TunerConstants.kPigeonId; kSteerInertia = TunerConstants.kSteerInertia; kDriveInertia = TunerConstants.kDriveInertia; - kSteerFrictionVoltage = TunerConstants.kSteerFrictionVoltage; - kDriveFrictionVoltage = TunerConstants.kDriveFrictionVoltage; + kSteerFrictionVoltage = 0.0; + kDriveFrictionVoltage = 0.1; kSteerCurrentLimit = 40.0; // Example from CTRE documentation kDriveCurrentLimit = 120.0; // Example from CTRE documentation kOptimalVoltage = 12.0; // Assumed Ideal @@ -137,7 +137,8 @@ public class DriveConstants { kFrontLeftDriveType = "kraken"; kFrontLeftSteerType = "kraken"; kFrontLeftEncoderType = "cancoder"; - kFrontLeftEncoderOffset = Units.rotationsToRadians(TunerConstants.kFrontLeftEncoderOffset); + kFrontLeftEncoderOffset = + -Units.rotationsToRadians(TunerConstants.kFrontLeftEncoderOffset) + Math.PI; kFrontLeftDriveInvert = TunerConstants.kInvertLeftSide; kFrontLeftSteerInvert = TunerConstants.kFrontLeftSteerInvert; kFrontLeftEncoderInvert = false; @@ -153,7 +154,7 @@ public class DriveConstants { kFrontRightSteerType = "kraken"; kFrontRightEncoderType = "cancoder"; kFrontRightEncoderOffset = - Units.rotationsToRadians(TunerConstants.kFrontRightEncoderOffset); + -Units.rotationsToRadians(TunerConstants.kFrontRightEncoderOffset); kFrontRightDriveInvert = TunerConstants.kInvertRightSide; kFrontRightSteerInvert = TunerConstants.kFrontRightSteerInvert; kFrontRightEncoderInvert = false; @@ -168,7 +169,8 @@ public class DriveConstants { kBackLeftDriveType = "kraken"; kBackLeftSteerType = "kraken"; kBackLeftEncoderType = "cancoder"; - kBackLeftEncoderOffset = Units.rotationsToRadians(TunerConstants.kBackLeftEncoderOffset); + kBackLeftEncoderOffset = + -Units.rotationsToRadians(TunerConstants.kBackLeftEncoderOffset) + Math.PI; kBackLeftDriveInvert = TunerConstants.kInvertLeftSide; kBackLeftSteerInvert = TunerConstants.kBackLeftSteerInvert; kBackLeftEncoderInvert = false; @@ -183,21 +185,23 @@ public class DriveConstants { kBackRightDriveType = "kraken"; kBackRightSteerType = "kraken"; kBackRightEncoderType = "cancoder"; - kBackRightEncoderOffset = Units.rotationsToRadians(TunerConstants.kBackRightEncoderOffset); + kBackRightEncoderOffset = -Units.rotationsToRadians(TunerConstants.kBackRightEncoderOffset); kBackRightDriveInvert = TunerConstants.kInvertRightSide; kBackRightSteerInvert = TunerConstants.kBackRightSteerInvert; kBackRightEncoderInvert = false; kBackRightXPosInches = TunerConstants.kBackRightXPosInches; kBackRightYPosInches = TunerConstants.kBackRightYPosInches; - kDriveP = TunerConstants.driveGains.kP; - kDriveI = TunerConstants.driveGains.kI; - kDriveD = TunerConstants.driveGains.kD; - kDriveF = TunerConstants.driveGains.kV; + // NOTE: The PIDF values from TunerConstants.java make WPILib/AK implemention go crazy + // These values are from the AK example sketches + kDriveP = 0.05; + kDriveI = 0.0; + kDriveD = 0.0; + kDriveF = 0.13; kDriveIZ = 0.0; - kSteerP = TunerConstants.steerGains.kP; - kSteerI = TunerConstants.steerGains.kI; - kSteerD = TunerConstants.steerGains.kD; - kSteerF = TunerConstants.steerGains.kV; + kSteerP = 7.0; + kSteerI = 0.0; + kSteerD = 0.0; + kSteerF = 0.0; kSteerIZ = 0.0; break; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 7ce51c18..f95ed38d 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -35,7 +35,7 @@ public class GyroIOPigeon2 implements GyroIO { // Constructor public GyroIOPigeon2() { - pigeon = new Pigeon2(kBackLeftEncoderId, kBackLeftEncoderCanbus); + pigeon = new Pigeon2(kPigeonId, kCANbusName); pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); yaw = pigeon.getYaw(); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index b7008711..b6180dfa 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -78,7 +78,7 @@ public ModuleIOTalonFX(int index) { turnTalon = new TalonFX(kFrontRightSteerMotorId, kFrontRightSteerCanbus); cancoder = new CANcoder(kFrontRightEncoderId, kFrontRightEncoderCanbus); absoluteEncoderOffset = new Rotation2d(kFrontRightEncoderOffset); - isTurnMotorInverted = DriveConstants.kFrontRightSteerInvert; + isTurnMotorInverted = kFrontRightSteerInvert; break; case 2: @@ -87,7 +87,7 @@ public ModuleIOTalonFX(int index) { turnTalon = new TalonFX(kBackLeftSteerMotorId, kBackLeftSteerCanbus); cancoder = new CANcoder(kBackLeftEncoderId, kBackLeftEncoderCanbus); absoluteEncoderOffset = new Rotation2d(kBackLeftEncoderOffset); - isTurnMotorInverted = DriveConstants.kBackLeftSteerInvert; + isTurnMotorInverted = kBackLeftSteerInvert; break; case 3: @@ -96,7 +96,7 @@ public ModuleIOTalonFX(int index) { turnTalon = new TalonFX(kBackRightSteerMotorId, kBackRightSteerCanbus); cancoder = new CANcoder(kBackRightEncoderId, kBackRightEncoderCanbus); absoluteEncoderOffset = new Rotation2d(kBackRightEncoderOffset); - isTurnMotorInverted = DriveConstants.kBackRightSteerInvert; + isTurnMotorInverted = kBackRightSteerInvert; break; default: From 4ae53e9ffdcdfa1b5aa82fcd51d12b74c3f9f26d Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 13 Dec 2024 11:56:24 -0700 Subject: [PATCH 57/57] Remove pubish_release action deleted: .github/workflows/pubish_release.yml --- .github/workflows/pubish_release.yml | 15 --------------- 1 file changed, 15 deletions(-) delete mode 100644 .github/workflows/pubish_release.yml diff --git a/.github/workflows/pubish_release.yml b/.github/workflows/pubish_release.yml deleted file mode 100644 index 6f9abb92..00000000 --- a/.github/workflows/pubish_release.yml +++ /dev/null @@ -1,15 +0,0 @@ -name: Publish - -on: - push: - tags: - - "v*" - -jobs: - build: - runs-on: ubuntu-latest - steps: - - name: Checkout - uses: actions/checkout@v4 - - name: Release - uses: softprops/action-gh-release@v2